SLAM algorithm with ultrasound range input implemented on a Crazyflie drone
CrazySLAM implements a SLAM algorithm using ultrasound range inputs to localize the Crazyflie drone. The project was conducted as part of my engineering degree at INSA Rouen Normandie.
The localization stack is the most important module in autonomous, mobile systems. Where in some cases the solution to this problem is pretty straightforward (applications with a high inaccuracy tolerance and high prior knowledge of an outdoor environment), it can get really difficult for some kind of devices.
One of the biggest challenges in localization is when the vehicle is operated in an indoor environment. Because it can't use GPS sensors, some of the remaining options are :
None of these options were good enough for the Crazyflie:
Simultaneous Localization And Mapping (or SLAM) is the computational problem of constructing the map of an unknown environment while simultaneously updating the position (or state estimate) of the vehicle. This is what a SLAM algorithm generally looks like:
In others words, the intuition behind this class of algorithms is : What motion best explains the difference between sensors observations at t-1 and t given what is known about the environment ?
To implement this method on the Crazyflie, a Kalman filter state estimate is used (for step 2) which is computed by the drone's firmware. It uses sensor measurements from the onboard IMU and gyroscope. For map updates and to correct the state estimate (step 1 and 3), the Multi-ranger deck that has 6 TOF ultrasound sensors is used.
Currently, the implementation only supports 2D localization. The state vector contains the position along the x axis, y axis, and the yaw (or heading).
The map is represented as an occupancy grid. It's a 2D array where the value in the (i, j) cell is the probability of it being occupied. But keeping track of probabilities directly can be hard (because of some mathematical constraints). Instead of using occupancy probability, the occupancy log odds are used.
The odds of an event are the ratio of the probability of the event happening over the probability of the event not happening. Because of the characteristics of the log function, the computation for map updates then becomes additions of the log odds.
The update rules for a cell in a 2D grid map are :
grid[i, j] += LOG_ODD_OCC
grid[i, j] -= LOG_ODD_FREE
The values of the log odds (LOG_ODD_FREE
and LOG_ODD_OCC
) will be fixed
parameters of the model. We'll clip the log odds values to minimum and
maximum values (LOG_ODD_MIN
and LOG_ODD_MAX
which will also be parameters)
as it's never good to be too sure about a cell being occupied or free.
With this representation, the update algorithm becomes relatively simple:
The following simulations use data from the course on robotics by the University Of Toronto on Coursera.
The data is a set of LIDAR scans from a moving robot with accurate state estimations. On the following figures, the environment is mapped using 1000, 100, 10, then only 4 LIDAR points from each scan. This is to demonstrate the loss of information related to a smaller number of data points (only 4 TOF sensor are used on the Crazyflie).
The implementation of this module is fully vectorized, which allow high frequency map updates. The following frequencies were measured using an Intel Core i5 CPU, with 8 cores.
Number of data points in each scan | Update frequency |
---|---|
1000 | 50 Hz |
100 | 400 Hz |
10 | 1200 Hz |
4 | 1400 Hz |
The localization module answers the question that was asked earlier :
What motion best explains the difference between sensors observations at t-1 and t given what is known about the environment ?
Because we use inaccurate sensors, there is a lot of noise in the state estimate. A Particle Filter models this noise with a Gaussian representation. Instead of keeping only one state estimate, it use a big number of particles, with every particle representing a possible state of the vehicle with a corresponding weight (the bigger the weight, the most likely the particle represents the ground truth state).
At each timestamp, it looks at the sensors range inputs and the map. To update each particle's weight, it computes a correlation score : Given the position estimate of the particle, is there any compatibility between what the sensors "see", and what they're supposed to "see". In other words, are the targets on the sensors observations occupied cells on the map.
Algorithm :
To ensure that all the particles are still relevant, the particles are re sampled at the end of each iteration if the number of effective particles is lower than a fixed threshold.
The following simulations also use the Coursera's data. Because it don't contain any IMU or odometry readings for the motion model update, we'll just use random walk.
On the next figures, the state estimates are made using a fixed number of data points on each scan but a variable number of particles
With a small number of particles, the algorithm seems to get lost along the way. The fist half positions are accurate but then the error grows exponentially. The downside of using random walk is that it involves a lot of luck. This will not be a problem for our implementation as we'll use the state estimate given by the onboard Kalman Filter algorithm.
With a big enough number of particles, the error is nearly non existent.
On the next figures, the state estimates are now made using a fixed number of particles but different numbers of data points on each scan.
With just 4 data points on each scan (what is available on the Crazyflie), the algorithm performs badly and don't seem to find the correct path with 500 particles.
The experiments showed that it is not the case with a bigger number of particles (+1500), but this poses another issue : performance.
The previous simulations gives us the following performances :
Number of data points on each scan | Number of particles | Update frequency |
---|---|---|
100 | 100 | 740 Hz |
100 | 500 | 180 Hz |
100 | 1000 | 13 Hz |
Number of data points on each scan | Number of particles | Update frequency |
---|---|---|
4 | 500 | 360 Hz |
100 | 500 | 180 Hz |
1000 | 500 | 25 Hz |
Given the two previous modules, the SLAM algorithm is quite simple:
The first set of figures shows the ground truth map on the left, the map with noisy state estimates in the middle and the one with the SLAM algorithm on the left. We see excellent results for the SLAM algorithm, even though the orientation is skewed anti-clockwise.
The following figures shows the path plotted on the ground truth map. The path is correct but seems to be off on the lower side of the map.
Finally, the error analysis (difference between the slam state vector and the ground truth) shows that globally the SLAM algorithm succeeds at correcting the state estimations :
The biggest problem however is performance. The previous simulation ran with 3000 particles and 1000 data points and took around an hour. With an update speed of only 3 Hz, it can't (for the moment) be used in real time.
git clone https://github.com/khazit/CrazySLAM.git
cd CrazySLAM
pip install .