Using robotic simulator for prediction step in probabilistic localization approaches

Question

Probabilistic localization approaches like Kalman or Monte Carlo benefit from an accurate prediction step. The more accurate the prediction step, the more accurate is the belief of the robots pose. In most approaches probabilistic motion models are applied, mainly because robot dynamics are more difficult to model. Still some approaches rely on dynamic models in order to increase the accuracy.

Therefore, I was wondering if it’s reasonable to utilize a robotic simulator like V-REP or Gazebo for the prediction step. The advantages I see in doing so are the following:

• the robots kinematic is solved by default, simply through modeling it in the robotic simulator
• the robots dynamics are taken into account
• nonlinear behaviors like slippage or collision can be modelled up to a certain extend
• the robots workspace is taken into account, by modeling its environment (if the robot drives against a wall previous models would predict it behind the wall, which won’t happen in a robotic simulator)

With the shown advantages I hope to achieve a more accurate prediction.

However there might be some problems using a robotic simulator. For a start it has to ensure real time behavior and there will be delay in the prediction due to the communication with the simulator.

I was looking for some papers which pick up on that idea but couldn’t find any. Are there any approaches similar to my idea? If not, are there any reasons why nobody is using a robotic simulator for the prediction? What are your opinions about my proposal?

Answer 1

In the probabilistic approaches that you mentioned, specifically Monte Carlo Localization (MCL), the prediction step is made based on state (pose) information and measurements. MCL relies on the Markov Assumption, which is the assumption that at each time step, the knowledge of previous time steps contains no more information than knowledge of the current state. Motion and measurement models use the state and measurement information to make the prediction; these models are similar, if not the same, as those used in simulations.

To answer your question, using a full-blown simulation environment is a very slow way of predicting the state because the underlying models are the same (motion models include the dynamics, and measurement models include sensor noise), but the computation time needed to simulate the system is greater when using the simulator. In short, nothing new is gained by using a simulation environment, and it takes longer.

Probabilistic Robotics by Sebastian Thrun gives this material very good treatment.

Answer 2

You are spot on by stating that a full-blown simulator would take into account physical interactions with the environment, e.g. "forbidding" the robot to get into a wall.

However, something like a kalman filter requires the prediction step to both predict the mean and the covariances of the probability distribution, something that general simulators do not do.

Using such a simulator would be doable within a particle filter, though, and might be beneficial if the prediction heavily depends on complex interactions with the environment.

Answer 3

Probabilistic localization is normally done with a hypothesis tracker aka particle filter. To use this approach together with a physics-engine, different engines of Box2D has to used in parallel (one instance for one particle). To generate a new instance the following code-snippet will work:

for i in range(len(self.engine[0].joint)):
  self.engine[destination].joint[i].motorSpeed = self.engine[source].joint[i].motorSpeed
  self.engine[destination].joint[i].bodyA.angularVelocity = self.engine[source].joint[i].bodyA.angularVelocity
  self.engine[destination].joint[i].bodyB.angularVelocity = self.engine[source].joint[i].bodyB.angularVelocity
for i in range(len(self.engine[0].body)):  
  self.engine[destination].body[i].position = self.engine[source].body[i].position
  self.engine[destination].body[i].angle = self.engine[source].body[i].angle
  self.engine[destination].body[i].linearVelocity = self.engine[source].body[i].linearVelocity
  self.engine[destination].body[i].angularVelocity = self.engine[source].body[i].angularVelocity

Physics based localization is not discussed in literature, but Physics based control:

• DANCE: Faloutsos, Petros and Van de Panne, Composable controllers for physics-based character animation (2001)
• MuJoCo: Todorov, Emanuel and Erez, Tom, A physics engine for model-based control, 2012