# Geometric model for differential wheeled mobile robot

In the following, the geometric model is a mathematical transformation where the inputs are the angular velocities of the wheels (usualy measured with encoders) and the output is the pose (position and orientation) of the mobile robot in the working space.

## Problem specification

We focus here on a differential wheeled mobile robot. Such robot is composed of two wheels aligned on the same axis. Here is an illustration of Rat-Courci, a small differential wheeled robot designed for the Micromouse competition:

The wheel diameter is given by $D=2.r$ where $r$ is the radius. The distance between the center of the robot and the wheels is given by $l$, the distance between wheels is $2.l$ according to the following illustration:

We'll assume that the following parameters are known:

• $r$ is the radius of the wheels

• $l$ the distance between the center of the robot and the wheels

• $\omega_l$ and $\omega_r$ are the instantaneous angular velocities of respectively the left and right wheels

Our goal is to calculate the pose of the robot according to the upper figure:

• $x$ and $y$ are the coordinates of the robot

• $\psi$ is the angular orientation of the robot

## Elementary displacement calculation

First, let's calculate the linear velocity of each wheel:


\begin{array}{r c l}
v_l &=& r.\omega_l \\
v_r &=& r.\omega_r
\end{array}


The average velocity of the robot is then given by:

The robot velocity can now be projected along the $x$ and $y$ axes:


\begin{array}{r c l}
\Delta_x &=& v_{robot}.cos(\psi)&=&\frac {r}{2} ( \omega_l.cos(\psi) + \omega_r.cos(\psi))\\
\Delta_y &=& v_{robot}.sin(\psi)&=&\frac {r}{2} ( \omega_l.sin(\psi) + \omega_r.sin(\psi))
\end{array}


The angular velocity of the robot is given by the difference of the wheels linear velocities:

Previous equation can be reformulated as:

The elementary displacement of the robot is given by the following relation:


\left[ \begin{matrix} \Delta_x \\ \Delta_y \\ \Delta_{\Psi} \end{matrix} \right]=
\frac{r}{2}.
\left[ \begin{matrix}
cos(\psi) & cos(\psi) \\
sin(\psi) & sin(\psi) \\
\frac{1}{l} & \frac{-1}{l}
\end{matrix} \right].
\left[ \begin{matrix} \omega_r \\ \omega_l\end{matrix} \right]


## Absolute position

The absolute position can be calculated thanks to the following equations :

\begin{array}{r c l}
x_{i}&=&x_{i-1}+\Delta_x \\
y_{i}&=&y_{i-1}+\Delta_y \\
\Psi_{i}&=&Psi_{i-1}+\Delta_{\Psi}
\end{array}

where

• $x_{i}$ and $y_{i}$ are the coordinates of the robot at time step $i$

• $\Psi_{i}$ is the orientation of the robot at time step $i$

Of course, this model has some limitations. The result is highly dependant on the accuracy of the robot (mechanical assembly, wheel diameter, distances ...). We assume that wheels rotate without any slippery problems which is not true in practice. We also assume that the sampling rate is fast enough to assume that $\Delta_x$, $\Delta_y$ and $\Delta_\Psi$ will remain elementary displacements.

# Arduino-based accurate distance measurement with Sharp sensors.

The aim of this post is to explain how to convert voltage from Sharp ranging sensor into an accurate distance. Experiments have been performed with Sharp GP2Y0A02YK and GP2Y0A21YK sensors. The range of this triangulation-based sensors are respectively from 20cm to 150cm and from 10 to 80cm.

## Measurement survey

An accurate measurement survey has been performed. Range is mesasured with a step of 10mm. The following graph show the behavior of the GP2Y0A02YK sensor according to the distance. As it is explained in the sensor's specifications the sensor is not suited for measurement below 20cm. This range can even not be exploited due to the ambiguity.

Same methodology and graph for the GP2Y0A21YK sensor below:

## Polynomial approximation

Thanks to the Matlab polyfit fonction, the curv (for the GP2Y0A02YK) has been approximated with a fourth degree polynomial. The approximation covers the range from 14cm to 150cm. The following figure shows the result.

For the GP2Y0A21YK a fifth degree polynomial was necessary:

## Results

The approximation for the GP2Y0A02YK is given by the following equation. $ADC$ is the raw value returned by the Arduino analogRead() function.

 Distance =  2583.711122992086 - 20.197897855471.ADC + 0.071746539329.ADC^2 -0.000115854182.ADC^3 +0.000000068590.ADC^4


The same approximation for the GP2Y0A21YK is given by:

 Distance =  200.3775040589502 -2.2657665648980.ADC + 0.0116395328796 .ADC^2 -0.0000299194195.ADC^3 + 0.0000000374087 .ADC^4  -0.0000000000181.ADC^5


Based on this result, a C++ function can easily be written in order to compute the distance:

/*!
\brief  make a distance measurement with a Sharp GP2Y0A02YK sensor
\return the measured distance in mm

*/
float get_Sharp_GP2Y0A02YK_Distance(int PinID)
{
// Read analog to digital converter value

// Convert in millimeters and return distance
return  2583.711122992086
}


For the GP2Y0A21YK :

/*!
\brief  make a distance measurement with a Sharp GP2Y0A21YK sensor
\return the measured distance in mm

*/
float get_Sharp_GP2Y0A21YK_Distance(int PinID)
{
// Read analog to digital converter value

// Convert in millimeters and return distance
return  200.3775040589502