Interlocalisation - Introduction
Interlocalisation is the feature allowing a HiveBoard-BeeBoard assembly to calculate its distance and angle to other HiveBoards. The distance is defined as the norm of the straight 3D line connecting two HiveBoards (if there is an elevation difference between both boards, this will show up in the distance measurement) while the angle (sometimes called azimuth) is the relative bearing of another HiveBoard with regards to the first. No elevation measurement is done, the system can therefore only work properly on a 2D plane. The following figure shows those two concepts with the distance marked \(r\) and the angle \(\theta\).
Info
The 0° location of a BeeBoard assembly is chosen during calibration. This way, it is always possible to align the 0° of the interlocalisation engine with the front of the robot on which it is mounted.
All measurements done by the interlocalisation engine are made available through Buzz's neighbor structure or via the HiveMind API.
The following page gives a brief overview of how the system is organized from a hardware and software point of view. Other pages explain how distances are calculated, how angles are calculated and how all messages are synchronized between all HiveBoards.
Hardware
The interlocalisation feature is built around Decawave's DW1000 UWB chip. Each BeeBoard contains a single DW1000. A single BeeBoard is needed to calculate distances between two systems. On the other hand, in order to calculate angles, three BeeBoards must be plugged into a HiveBoard. One BeeBoard should be plugged into each of the HiveBoard's channels (A, B and C). Whether the BeeBoard is plugged into port 0 or 1 has no impact.
Note
The BeeBoard plugged into channel B will act as the master in all exchanges. This means that distances and angles will be calculated relative to that specific BeeBoard. Because the boards are so close to each other, this has practically no impact.
The BeeBoards must then be placed in a specific pattern using the holder found in the HiveBoard-BeeBoard-Mechanical GitHub repository and the whole system must be calibrated.
Important
Calibrations are very sensitive to all elements involved in the assembly. Therefore, a new calibration must be done for any change in components (HiveBoard, BeeBoard, USB-C cable, port in which the BeeBoard is plugged, etc.)
Software Architecture
In the HiveMind firmware, interlocalisation is split between multiple threads and libraries.
The measurements themselves are made in the BSP library. The BSP library is different for each compilation target. For a microcontroller build, the library is responsible for sending/receiving messages and calculating a distance and an angle. While for a ROS build, the library gets interlocalisation information from Gazebo and doesn't have any communication to handle.
Measurements made in this low-level library are then pushed to the InterlocUpdateInputQueue. This queue feeds the Interloc library which is a high-level library responsible for filtering and storing interlocalisation data. Each time an update is pushed to the queue, a thread picks it up, filters it and updates the specific neighbor's position in the PositionsTable. A message is also sent to Buzz via the InterlocUpdateOutputQueue to notify it that an updated position is available for a given neighbor.
Filtering of values is done using a very simple exponential moving average filter. $$ FilteredValue_t = \alpha \cdot UnfilteredValue_t + (1-\alpha) \cdot FilteredValue_{t-1} $$
Note
For better results, this filter should be changed for something like a Kalman filter or a particle filter.
Usage
As shown in the previous figure, interlocalisation data can be accessed via Buzz's neighbor structure or the MessageHandler (HiveMind API).
The message handler allows accessing more than the current calculated positions. It is possible to get the raw data used to calculate angles or get periodic dumps of all calculated positions. These features are mainly used for calibration and characterization purposes and examples on their usage can be found in the TestBench-Python repository.