Localising a robot placed at an unknown position in a known environment

Question

I am a third-year electrical engineering student and am working on an intelligent autonomous robot in my summer vacations.

The robot I am trying to make is supposed to be used in rescue operations. The information I would know is the position of the person (the coordinates of the person in a JSON file) to be rescued from a building on fire. I would also know the rooms of the building from a map, but I don't know where the robot may be placed inside the building to start the rescue operation.

That means I have to localise the robot placed at an unknown position in a known environment, and then the robot can plan its path to the person who has to be rescued. But, since this is not my domain I would like you to guide me on what is the best method for localising given that I can use an IMU ( or gyro, accelerometer, magnetometer) and ultrasonic sensors to do the localising job. I cannot use a GPS module or a camera for this purpose.

I, however, do know how to do path planning.

As far as my research on the Internet is concerned I have found a method called "Kalman filtering" that maybe can do the localising job. But there are I think some other filtering methods as well. Which one should I use? Or is there any other simpler/better method out there of which I don't know yet?

I am also attaching the map of the building which is known to me.

Edit:

The terrain is flat, and I would like to know where the robot is on the map like at coordinate 0,4 etc.

Answer 1

The problem is that you can't apply path planning until you know where the robot is in the global coordinate frame. There are many localization techniques, and each has its pros/cons; I have used Particle Filtering for a very similar localization task. Extensive coverage of particle filtering is given by Sebastian Thrun in his book Probabilistic Robotics--see chapter 8. There are advantages of particle filtering over Kalman Filtering (and the other way around), but the nice thing about particle filtering is that you can use raw measurements directly, it accepts measurement noise with any distribution, it can be used for global localization, it doesn't rely on a discretization of the world (e.g. a grid), and it is relatively simple to implement.

The algorithm presented by Thrun is Monte Carlo Localization (MCL). Prerequisites for the algorithm are that you have a model for the motion of your robot (this could be noisy unicycle dynamics) and a measurement model of your sensor--in your case, the ultrasonic sensor is a noisy beam sensor. See my previous post about ultrasonic sensors and measurement models. For the particle filter, you won't really need the IMU data, but you can incorporate it to improve your localization results; I will assume that you won't do this.

The MCL algorithm is summarized but the following pseudo-code:

1. $\bar{X}=X=\varnothing$

2. For m=1 to M particles, do:

   i. $x_t^{[m]}$=sample_motion_model($u_t,x_{t-1}^{[m]}$)

   ii. $w_t^{[m]}$=sample_measurement_model($z_t,x_t^{[m]},m$)

   iii. $\bar{X_t}+=\langle x_t^{[m]}, w_t^{[m]}\rangle$

3. For m=1 to M particles, do:

   i. sample particle $i$ with probability proportional to $w_t^[i]$

   ii. add $x_t^{[i]}$ to $X_t$

4. Return $X_t$

$X_t$ is the set of M particles $\{x_t^{[1]},x_t^{[2]},...,x_t^{[M]}\}.$ $u_t$ is the action taken at time $t$, $z_t$ is the measurement taken at time $t$, $x_t^{[m]}$ is a particle representing the state of the robot at time $t$, and $w_t^{[m]}$ is the weight of particle $m$ at time $t$. The bar notation indicates a belief prior to incorporating the measurement, just after executing the action.

In words, particle filtering works like this: throw out a bunch of particles, see how good they are based on what the robot has just done and based on what the robot observes, then pick the best particles and repeat. For particle filtering, you need to use a sufficiently large number of particles, and there are numerous techniques that can be used to improve performance and accuracy. Also, you need to know some math to go through the proof, but it is straightforward.

Answer 2

There are many tactics for localization in a known environment. the most popular ones are the Filtering methods which include Kalman filtering and Particle filtering. Nowadays the second one is the most efficient and most widely used in this kind of situations. A good starting point for learning about localization techniques and the intuition behind them is this Robotics course on Udacity by Sebastian Thrun.

Answer 3

Firstly, to solve your problem in full would be a rather large post so I will focus on the methodology that you should follow.

As others have said, you would use some kind of filtering method to solve your problem; particle filtering, Kalman filtering, they are all the same thing but different variants that have different strengths. You could also use Markov Model methods that operate in discrete space rather than continuous space.

Firstly let's define the task: 3D (X, Y positions and yaw angle) localisation using an a priori map. This is different from SLAM (Simultaneous Localisation and Mapping) because you already have a map. That being said, you could probably use an existing slam algorithm with a high-belief prior set by the map you have.

This leads to the definition of the states you are interested in (called the state vector): $$\mathbf{x}_{k} = \big[ \mathbf{p}^\top, \mathbf{v}^\top, \psi \big]^\top$$ Where $\mathbf{x_k}$ is the state vector at discrete time-step $k$, $\mathbf{p}$ and $\mathbf{v}$ are the position and velocity, and $\psi$ is the yaw angle of the robot (all measured with respect to, and expressed in a global frame $\{G\}$). Velocity is added there because it is necessary for the prediction stage of the filter.

Second let's define the observations you can make (I exclude sensor measurements unnecessary for solving this 3DOF problem):

• Range measurements of the relative position to the wall ahead of the robot: $y_u(\mathbf{x_k})$
• Inertial measurements of the motion of the body:
  • Acceleration (2 axes, x and y): $\mathbf{a}_k$
  • Angular rotation rate (1 axis, z) $\mathbf{\omega}_k$
  • Magnetic field strength (1 axis, z) $m_k$
• All measurements are made in the body frame $\{B\}$ which is attached to the IMU (note there is a transform between the ultrasound and body which I am not addressing, but should be addressed in an actual solution).

Now the issue is how do you use a filtering methodology to localise your robot? Regardless of your choice of filter you will need a model that describes how the robot evolves through time (the process model). Then you need a measurement model for each of your sensors, which describes what measurement is expected given an arbitrary state $\mathbf{x}$ of the robot.

For such a problem, your measurement models would describe (1) the ultrasound range, (2) the accelerometer, (3) the rate gyro, (4) the magnetometer.

Because of the high update rates of the IMU it is commonly used in the process model directly so that the update stage of the filter is not used too frequently (it is more computationally expensive than the predict stage). I'll refer you to equations (3) - (8) in this paper for this process model (in this application there is no need for quaternions so I'd advise to use Euler angle representations of the orientation instead).

Additionally, you must model the bias states in the IMU, which then adds another two states to your state vector: $$\mathbf{x}_{k} = \big[ \mathbf{p}^\top, \mathbf{v}^\top, \psi, \mathbf{b}_a^\top, b_\omega \big]^\top$$ Where $\mathbf{b}_a$ and $b_\omega$ are the biases in the accelerometers and gyroscope, respectively.

The measurement model for the magnetometer can be found in equation 5.10 in this thesis.

Let's simplify your measurement model for your ultrasound, and pretend it is a laser range finder that simply returns the distance of the object (wall) it is pointing to. I assume you can do the calculation to convert the pulse times to distance. Your model is then: $$y_u(\mathbf{x}_k) = |\mathbf{p}_{w,B}^B|_2 = |R^B_G(\mathbf{p}_{w,G}^G - \mathbf{p}_{B,G}^G)|_2$$ Where $ \mathbf{p}_{w,B}^B $ is the position of the wall that is being pointed to, with respect to (superscript) and measured from (subscript) the $\{B\}$ frame. $R^B_G$ is the rotation matrix that rotates a vector in the $\{G\}$ frame to the $\{B\}$ frame, and $\mathbf{p}_{B,G}^G$ is the position of the $\{B\}$ frame with respect to and expressed in the $\{G\}$ frame (which is the robot location represented by $p$ in the state vector).

You'll have to think about how to parameterize and calculate $\mathbf{p}_{w,G}^G$ (which is a function of the wall geometry, $p$ and $\psi$), I would think it would work with a look-up table or a solution to a set of line intersection equations representing the range beam and the walls. Either way it is discontinuous and nonlinear so this supports the use of a particle filter over a Kalman-based filter. You could possibly have a different measurement model for each wall if you really want to use a Kalman Filter, but that introduces a new data-association problem which is non-trivial.

I hope this plus NBCKLY's PF psuedocode gets you on your way.

A good resource on IMU models and noise.

Maybe of interest is this use of highly non-linear maps in PF's.