Determining Position Using Monte Carlo Localization

We now have several interesting sensors on our robot. However, we have yet to combine them to understand the position of our robot. The Monte Carlo simulation is a method that uses multiple sensors and a model of a robot’s world to estimate its location and heading in that world.

You will learn how to make a test arena for a robot, followed by how to model this arena in code, and how to send this data over Bluetooth to view on a computer. You will practice statistical methods for the robot to start guessing its location. You will see how to enrich encoder data and move the guesses, and then integrate this with distance sensor data to refine the guesses, using a method that is effective in the face of noisy sensor data and can cope with minor inaccuracies. This will come together in a Monte Carlo guess and check loop.

In this chapter, we will cover the following main topics:

• Creating a training area for our robot...

Technical requirements

For this chapter, you will require the following:

• The robot and code from Chapter 12, Detecting Orientation with an IMU on Raspberry Pi Pico
• A PC or laptop with Bluetooth LE
• Python 3.7 with the Python matplotlib, bleak, and NumPy libraries installed
• 10 x 10-mm A1 sheet foam boards
• Duct or gaffer tape
• A tape measure
• A metal ruler, set square, and pencil
• A sharp craft knife
• A floor space of 1.5 sq meters

You can find the code for this chapter at https://github.com/PacktPublishing/Robotics-at-Home-with-Raspberry-Pi-Pico/tree/main/ch-13.

Creating a training area for our robot

We will be estimating a robot’s location in a space. The robot needs a known space to work in, so we will build a simple world for it to operate in. This training area, or arena, is loosely based on those used in Pi Wars (see https://piwars.org/2022-competition/general-rules/ under Arena construction rules), a British robotics competition, where this algorithm could be used for a robot to compete autonomously.

Let’s take a closer look at the arena.

What we will make

The following diagram shows the arena we will make:

Figure 13.1 – A robot test arena

Figure 13.1 shows a top-view drawing of an arena, complete with dimensions. The arena is mostly square to keep it simple to make and model. To help the Monte Carlo simulation work, there must be a cutout on one side to prevent rotational symmetry – that is, you can’t rotate the arena and have it appear identical from multiple angles...

Modeling the space

The aim of a Monte Carlo system is to model or simulate a space and a robot’s location. In this section, we will learn how code for the robot will represent this space. We will also look at how a computer can be used to visualize our robot’s guesses. Monte Carlo-based behavior code checks sensor readings frequently against the model of the space, so we should represent the space on the robot to optimize this.

The role of the computer and the robot in this are shown in the following diagram:

Figure 13.4 – Visualizing with the computer

Figure 13.4 shows an overview of this system’s display and control architecture. The behavior code runs on the robot. The computer displays the state of the robot code, along with start and stop controls. The arena and state of the system all belong to the robot.

Let’s look at how to represent the arena on the robot.

Representing the arena and robot position as numbers...

Using sensors to track relative pose

In this section, we will explore what a pose is, how to create, send, and display poses, and how to move the poses relative to the movement of the robot.

Setting up poses

We’ll make some random poses in robot/code.py using NumPy, a numeric manipulation library for fast array operations, with ulab providing this functionality in CircuitPython. This library also gives us handy ways of storing and dealing with arrays.

Import the ulab library, and random to generate random poses:

import asyncio
import json
import random
from ulab import numpy as np

After the read_json function, we’ll add a Simulation class to hold the poses:

class Simulation:
    def __init__(self):
        self.population_size = 20
        self.poses = np.array(
            [(
   ...

Monte Carlo localization

Our robot’s poses are going outside of the arena, and the distance sensor readings should show which guesses (poses) are more likely than others. The Monte Carlo simulation can improve these guesses, based on the sensor-reading likelihood.

The simulation moves the poses and then observes the state of the sensors to create weights based on their likelihood, a process known as the observation model.

The simulation resamples the guesses by picking them, so those with higher weights are more likely. The result is a new generation of guesses. This movement of particles followed by filtering is why this is also known as a particle filter.

Let’s start by giving our poses weights, based on being inside or outside the arena, and then we’ll look at how to resample from this.

Generating pose weights from a position

The initial weight generation can be based on a simple question – is the robot inside the arena or not? If not...

Summary

In this chapter, we started by building a test arena for our robot using foam board construction, then modeled this in code, and displayed it along with a distance sensor likelihood field. We put this on the robot, sent it over BLE, and then added poses.

We modeled how poses move using sensors, adding uncertainty to the model. We then added a model of distance sensor observations, generating weights that we used in a resampling algorithm to generate new poses.

We finished with a look at tuning factors to improve the performance of this system.

In the next chapter, we will summarize your Raspberry Pi Pico robotics learning journey so far and discuss how you can continue your journey by improving this robot or building more robots.

Exercises

The following exercises will deepen your understanding of the topics discussed in this chapter and make the robot code better:

• The IMU could be added by storing a previous state and calculating the delta. You could mix this into the rot1/rot2 values by taking the average of encoder calculations versus the IMU angles, or consider whether one sensor is more trusted than the others. You will need to calibrate the IMU before it can be used.
• The robot’s pose guesses get stuck in local maxima – good but wrong guesses that are likely based on sensor positions. Consider throwing in 10 fresh guesses at every population to nudge the code to try other options.
• We are using only two observations per pose – having more distance sensors could improve this but will make the model slower.
• Could you add a target zone to the arena? Consider how PIDs could be used to steer the robot toward this. Perhaps feed the PID with the mean pose.
• You can improve...

Further reading

These aids for further study will let you read on and dive deeper into the Monte Carlo algorithm and its quirks:

• Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard, and Dieter Fox, published by MIT Press, covers the Monte Carlo particle filter, along with the Kalman filter and other probability-based models in far more depth.
• I strongly recommend the Khan Academy material on modeling data distributions for learning and practicing data distributions.
• A playlist of 21 videos from Bonn University and Cyrill Stachniss at https://www.youtube.com/playlist?list=PLgnQpQtFTOGQEn33QDVGJpiZLi-SlL7vA covers the topics used here in detail. I recommend them if you want to dive far deeper into this topic.