Probabilistic Robotic Reading Note(2) - Velocity Motion Model

Subscribe Send me a message

Introduction

There are two steps in Bayes Filter. The first step is prediction: given the current state of the robot and the control applied to it, what will be the new state? The mathematical description of this step is the state transition probability:

$$ p(x_t|u_t, x_{t-1}) $$

Note that $x_{t}$ is conceptully unknown because it is the new state of the robot. However, when we calculate the state transition probability, $x_t$ becomes one of the arguments. Therefore, the value of $x_{t}$ is available when we calcuate the value of this probability.

This post consists of three small sections about the velocity motion model. First, we will describe the state transition of robot in a noise-free world. Then we will see how we model the noise in the velocity model. Finally, we will talk about the computation of the state transition probability.

Noise-free Velocity Model

In Velocity model, the control variable is the translational and rotational velocity of the robot:

$$ \begin{equation} u_t = \left( \begin{array}{c} v_t\\ w_t \end{array} \right) \end{equation} $$

In a noise-free world, the instantanuous movement of the robot should be a small arc on a circle. The radius of the circle is given by $r = \frac{v_t}{w_t}$. The figure below illustrates the state transition. The red arrows indicate the direction of the robot movement, which is tangent to the circle. In a perfect world, the rotation angle of the red arrow should be equal to $\Delta{}\theta$.

Fig-1: robot rotation

From here, we can establish the relationship between the new and old state. Let $(x_c, y_c)$ denote the center of the circle, we can show that

$$ \begin{eqnarray} x_c & = & x - r\sin{}\theta \\ y_c & = & y + r\cos{}\theta \end{eqnarray} $$

We can apply the same logic to the new state, which gives us

$$ \begin{eqnarray} x_c & = & x' - r\sin{}(\theta + w\Delta{}t) \\ y_c & = & y'+ r\cos{}(\theta + w\Delta{}t) \end{eqnarray} $$

Put them together, we get

$$ \begin{eqnarray} x' & = & x - \frac{v}{w}\sin{}\theta + \frac{v}{w}\sin{}(\theta + w\Delta{}t) \\ y' & = & y + \frac{v}{w}\cos{}\theta - \frac{v}{w}\cos{}(\theta + w\Delta{}t) \\ \theta{}' & = & \theta{} + w\Delta{}t \end{eqnarray} $$

Model the Noise

The Velocity motion model incorporates the following two noises

1. The control applied to the robot is not perfect and the difference between the control command and what robot actually does is assumed to be a Gaussian. The uncertainty will depend on the translational and rotational velocity.

$$ \begin{equation} \left( \begin{array}{c} \hat{v}_t\\ \hat{w}_t \end{array} \right) = \left( \begin{array}{c} v_t\\ w_t \end{array} \right) + \left( \begin{array}{c} \epsilon_{\alpha_1v^2 + \alpha_2w^2}\\ \epsilon_{\alpha_3v^2 + \alpha_4w^2} \end{array} \right) \end{equation} $$

2. In additional to the rotation around the center of the circular trajectory, the robot itself roates.

$$ \Delta{}\theta = w\Delta{}t + \hat{\gamma}\Delta{}t $$

where $\gamma$ follows $\mathcal{N}(0, \alpha_{5}v^2 + \alpha_{6}w^2)$.

Calcualte State Transition Probability

When we calculate the state transition probability, we have both $x_{t-1}$ and $x_t$ available together with the control variable $u_t$. Here is the step to calcuate the probability:

1. From $x_{t-1}$ and $x_t$, we can compuate the implied velocity and the implied rotation angle (e.g. $\hat{v}_t$, $\hat{w}_t$, $\Delta\hat{\theta}_t$).

The detailed steps of the calcuation can be found in the book. The general idea is to first identify the center of the circular trajectory. The center of the circle needs to of course satisfy

$$ \begin{eqnarray} x_c & = & x - r\sin{}\theta \\ y_c & = & y + r\cos{}\theta \end{eqnarray} $$

In addition, we konwn it's on the bisector of the segment defined by the start and end position of the robot. Therefore, we can write

$$ \begin{eqnarray} x_c & = & \frac{x + x'}{2} + \lambda(y - y') \\ y_c & = & \frac{y + y'}{2} + \lambda(x' - x) \end{eqnarray} $$

We now have a two unknowns and two equations and we should be able to calcuate the coordinates of the center.

2. From $u_t$, we can calcualte $\Delta\theta = w\Delta{}t$.

Now that we have the implied value and the value specifiedi in the command, we can easily calcuate the probability.

Algorithm

The Velocity motion model algorith is given as the follow:

----- END -----

Send me a message Subscribe to blog updates

Want some fun stuff?