Developing Probabilistic Motion Model for Diff. Steer Robot

Consider the differential steer robot as shown in figure 1:

$\scriptsize{Figure 1}$

The robot has a local frame $\{R\} - (u,v)$ and inertial frame $\{I\} - (k,y)$ with the geometric center $c$ located at $(0,0)$ in $\{R\}$ or $(x,y)$ in $\{I\}$.

The direct kinematics, based on the assumption of no-slip and pure-role, in the local frame as given as:

(1)
\begin{align} \bar V_{R} = \underline{J}\dot{\bar{q}} \end{align}

or

$\left \{\begin{array}{c} V_{u} & V_{v} & w \end{array}\right\}_{R} = \left[\begin{array}{cc} \frac{r}{2} & \frac{r}{2} & 0 & 0 & -\frac{r}{d} & \frac{r}{d}\end{array}\right]\left\{\begin{array}{c} \dot\theta_{l} & \dot\theta_{r} \end{array} \right\}$

and in the inertial frame as,

(2)
\begin{align} \dot{\bar{X_{I}}} = \underline{R}_{I}^{R}\underline{J}\dot{\bar{q}} \end{align}

or

$\left\{\begin{array}{c} \dot{x} & \dot{y} & \dot{\theta} \end{array} \right\}_{R} = \left[\begin{array}{ccc} C\theta & -S\theta & 0 & S\theta & C\theta & 0 & 0 & 0 & 1 \end{array} \right]\left[\begin{array}{cc} \frac{r}{2} & \frac{r}{2} & 0 & 0 & -\frac{r}{d} & \frac{r}{d}\end{array}\right]\left\{\begin{array}{c} \dot\theta_{l} & \dot\theta_{r} \end{array} \right\}$

The differential steer robot is a non-holonomic system with constraints that can only be written in the instantaneous sense. Therefore, position of the platform is found through numerical integrations (here Euler integration) as:

(3)
\begin{align} X_{i} = X_{i-1} + [\underline{R}_{I_{i-1}}^{R} \times \underline{J}_{i-1}\dot{\bar{q_{i-1}}}(t_{i} - t_{i-1})] \end{align}

or

$x_{i} = x_{i-1} + [C\theta_{i-1}\frac{r}{2}(\dot{\theta_{l}} + \dot{\theta_{r}})_{i-1}\Delta t]$
$y_{i} = y_{i-1} + [S\theta_{i-1}\frac{r}{2}(\dot{\theta_{l}} + \dot{\theta_{r}})_{i-1}\Delta t]$
$\theta_{i} = \theta_{i-1} + [\frac{r}{d}(\dot{-\theta_{l}} + \dot{\theta_{r}})_{i-1}\Delta t]$

if the control input $\bar{u}$ is given as $\bar{u} = \{\dot{\theta_{l}}, \dot{\theta_{r}}\}$, or

(4)
\begin{align} \bar{X_{i}} = \bar{X_{i-1}} + [\underline{R}_{I_{i-1}}^{R}\bar{V_{R}}\Delta t] \end{align}

or

$x_{i} = x_{i-1} + [(C\theta_{i-1}V_{u} - S\theta_{i-1}V_{v})\Delta t]$
$y_{i} = y_{i-1} + [(S\theta_{i-1}V_{u} + C\theta_{i-1}V_{v})\Delta t]$
$\theta_{i} = \theta_{i-1} + [w_{i-1}\Delta t]$

if the control input $\bar{u}$ is given as $\bar{u} = \{V_{u},w\}$. $(V_{v}$ must be 0 to satisfy no slip assumption)

This represents an "exact" model of the kinematics of a differential steer robot, where "exact" is based on the following assumptions:

• No slip
• Pure roll
• Numerical integration

Now consider a model to predict the probability of being at location $X_{i}$ given $X_{i-1}$ & $u_{i}$.

$p(x_{i} | u_{i}, x_{i-1})$

(Note here the interchangeable use of X_{i} & x_{i})

Two elements to consider:

1. The approach:
• The approach will be to assume a value for $x_{i}$ and calculate the probability of $x_{i}$ as the product of the probability in each input component, each as a normal-distribution with mean calculated from a given $x_{i}$ value and variance based on fixed coefficients $\alpha$ as: $\prod_{\forall j} p(v_{i} - \^v_{i},\sum_{j=1}^{n}\alpha_{j}v_{j}^{2}$ with $p(x-\mu, \sigma^{2}) = \frac{1}{\sqrt{2\pi \sigma^{2}}}exp(\frac{1(x-\mu)^{2}}{2\sigma^{2}})$
2. Since the mobile robot direct kinematics problem does not fully span the output space, often an extra control variable is added to account for this possibility.

# Example 1

Consider the control input: $\bar{u} = (V_{u},w)$ (Note: add $V-{v}$ to the control input to fully map space) Then, the probability, $p(x_{i},| u_{i}, x_{i-1})$ is given as:

$p(x_{i}| u_{i}, x_{i-1}) =$
$p(V_{u} - \^ V_{u}, \alpha V_{u}^{2} + \alpha_{2}w^{2}) *$
$p(w-\^ w, \alpha_{3}V_{u}^{2}+\alpha_{4}w^{2}) *$
$p(V_{v} - \^ V_{v}, \alpha_{5}V_{u}^{2} + \alpha_{6}w^{2})$

with $p(x-\mu, \sigma^{2}) = \frac{1}{\sqrt{2\pi\sigma^{2}}}exp(-\frac{(x-\mu)^{2}}{2\sigma^{2}})$

given:

• $X_{i-1}$ - previous pose (known)
• $\bar{U_{i}}$ - control input (known)
• $X_{i}$ - hypothesized new pose

$\rightarrow \bar{U_{1}} = (V_{u},w), V_{v} = 0$ from (4) $\^{\bar{V}}=\underline{R}_{I_{i-1}}^{R}^{-1}[\frac{(\bar{X_{i}}-\bar{X}_{i-1})}{\Delta t}]$

or

$\^{V}_{u} = \frac{[C\theta(x_{i} - x_{i-1}) + S\theta(y_{i} - y_{i-1})]}{\Delta t}$
$\^{V}_{v} = \frac{[-S\theta(x_{i} - x_{i-1}) + C\theta(y_{i} - y_{i-1})]}{\Delta t}$
$\^{w} = \frac{(\theta_{i} - \theta_{i-1})}{\Delta t}$

and $(\alpha_{1} - \alpha_{6})$ stated values.

# Example 2

Assume $\bar{u}_{i} = \{\dot{\theta}_{l},\dot{\theta}_{r}\}$

In this case, the controller directly defines velocities of the left/right wheels. (Note: alternatively. that this could be considered a sensor model in which $\dot{\theta}_{l}, \dot{\theta}_{r}$ are data from odometry s.t., $\bar{Z}_{i} = \{\dot{\theta}_{l}, \dot{\theta}_{r}\}$)

For this example we will consider the former. As in the previous case, a full mapping in the kinematic space is needed so an additional control/sensor space variance is added, $\gamma$, to the kinematic model (2) as:

$\dot{\bar{X}}_{i} = \underline{R}_{I}^{R}\underline{J}^{*}\dot{\bar{q}}$

with

$\underline{J}^{*} = \left[ \begin{array}{cc} \frac{r}{2} & \frac{r}{2} & \frac{\gamma}{2} & \frac{\gamma}{2} & -\frac{r}{d} & \frac{r}{d} \end{array} \right]$

(Note that the additional parameter $\gamma$ has been added in a non-linear fashion.)

Now the probability $p(\bar{X}_{i}|\bar{U}_{i}, X_{i-1})$ is given as:

$p(\bar{X}_{i}|\bar{U}_{i}, X_{i-1}) =$
$p(\dot{\theta}_{l} - \^\dot{\theta}_{l}, \alpha\dot{\theta}_{l}^{2} + \alpha_{2}\dot{\theta}_{r}^{2}) *$
$p(\dot{\theta}_{r} - \^\dot{\theta}_{r}, \alpha_{3}\dot{\theta}_{l}^{2} + \alpha_{4}\dot{\theta}_{r}^{2}) *$
$p(0 - \gamma, \alpha_{5}\dot{\theta}_{l}^{2} + \alpha_{6}\dot{\theta}_{r}^{2})$

with

$p(x - \mu, \sigma^{2}) = \frac{1}{\sqrt{2\pi\sigma^{2}}}exp(-\frac{(x-\mu)^{2}}{2\sigma^{2}})$

with

• $\bar{X}_{i-1}$ - previous pose (known)
• $\bar{U}_{i}$ - control input (known)
• $X_{i}$ - hypothesized new pose
• $\^{U}_{i}$ - control input needed to achieve the hypothesized pose, $X_{i}$

solve as:

$J^{*}\^{\dot{q}} = R_{I-{i-1}}^{R}^{-1}\frac{(\bar{X}_{i} - \bar{X}_{i-1})}{\Delta t}$
$\^{\dot{q}} = J_{sub}^{*}^{-1}R_{I-{i-1}}^{R}^{-1}\frac{(\bar{X}_{i} - \bar{X}_{i-1})}{\Delta t}$

with

$\^{\dot{q}} = \{\^{\dot{\theta}}_{l}, \^{\dot{\theta}}_{r})^{T}$
$J_{sub}^{*} = \left[ \begin{array}{cc} \frac{r}{2} & \frac{r}{2} & -\frac{r}{d} & \frac{r}{d} \end{array} \right]$

and

$\gamma = 2 \frac{-S\theta(X_{i}-X_{i-1} + C\theta(y_{i} - y_{i-1})}{\Delta t (\^{\dot{\theta}}_{l} + \^{\dot{\theta}}_{r})}$

The coefficients $\alpha \righarrow \alpha_{6}$ determine variance in each control input parameter.

# Matlab

Below you can find some visual figures generated by the matlab code based on varying $\alpha \rightarrow \alpha_{6}$

$\scriptsize{Figure 2}$

$\scriptsize{Figure 3}$

$\scriptsize{Figure 4}$

=

$\scriptsize{Figure 5}$

$\scriptsize{Figure 6}$