1.3 Positionnement du robot

Le positionnement du robot est obtenu par odométrie, c'est à dire que la position est obtenue par intégration de petits déplacements. L'intérêt de l'odométrie est qu'elle est assez simple à mettre en oeuvre et qu'elle est fiable. Par contre, quand on intègre les déplacements, on intègre aussi l'erreur ce qui fait que l'erreur de position croît avec le temps.

Voyons maintenant comment on réalise concrètement ce positionnement. Comme je l'ai dit, le DSP vient lire régulièrement (toutes les 1 ou 10 ms) la position de chaque roue codeuse. Entre deux lectures, on peut savoir de combien s'est déplacée chaque roue codeuse et il faut à partir de cela en déduire la position du robot.

Figure 1.3: Positionnement du robot

Il y a en fait deux manières d'approximer la trajectoire élémentaire parcourue par le robot pendant ce temps $Te$ :

• En utilisant des segments de droites. On considère alors que le robot va en ligne droite pendant le temps $Te$ en gardant l'orientation qu'il avait au début de cet élément de trajectoire. Ceci revient à supposer que les deux roues ont une vitesse constante et identique sur cet élément de trajectoire. A la fin de cet élément de trajectoire, on corrige l'orientation du robot en fonction de la différence de distance parcourue par les deux roues.
• En utilisant des arcs de cercles. On considère alors que le robot se déplace et change d'orientation en suivant un arc de cercle pendant le temps $Te$. Ceci revient à considérer que chacune des roues a une vitesse constante sur la période $Te$ mais que les vitesses des 2 roues ne sont pas identiques.

Sur le robot, il n'y a que l'approximation par des segments qui a été implémentée et elle donnait de bons résultats. Par contre, l'approximation par des arcs de cercle a été réalisée en simulations sous MATLAB et elle n'a pas fait apparaître de différences notoires lorsque la fréquence d'échantillonnage $1/Te$ reste suffisamment élevée. Avec les valeurs utilisées sur le robot, on ne voit absolument aucune différence.

On présentera quand même ici les deux méthodes pour voir en quoi elles différent et pourquoi elles convergent lorsque $Te$ devient suffisamment petit. Par contre, il faut garder à l'esprit qu'aucune de ces méthodes n'est absolument exacte.

1.3.1 Approximation par des segments de droites

Appelons $\Delta d$ et $\Delta g$ les distances (en mm) parcourues respectivement par les roues droites et gauches entre deux lectures des LM soit un intervalle de temps $Te$. Connaissant la pose du robot à l'instant $n-1$, on cherche la pose à l'instant $n$. On emploie volontairement ici le terme "pose" en accord avec de nombreuses publications. En effet, la pose représente à la fois la position du robot $(x,y)$ et son orientation $\theta$. En général, on utilise dans la littérature le terme "position" pour le doublet $(x,y)$, le terme "pose" pour la triplet $(x,y,\theta)$ et le terme "posture" $(x,y,\theta,\kappa)$ où $\kappa$ représente le courbure. Il faut remarquer que le terme position est souvent utilisé abusivement ^1.7 pour désigner un état du robot qui peut intégrer bien d'autres paramètres que sa position (x,y).

Figure 1.4: Approximation par des segments de droite

On a donc :
$\Delta moy_n = \frac{\Delta d_n + \Delta g_n}{2}$
$\Delta dif_n = \Delta d_n - \Delta g_n$
$\Delta x_n = \Delta moy_n \cos{\theta_{n-1}}$
$\Delta y_n = \Delta moy_n \sin{\theta_{n-1}}$
$\Delta \theta_n = \frac{\Delta dif_n}{L}$

En effet, lorsque le robot fait un tour sur lui-même ($2\pi$ radians), la différence de parcours entre les deux roues est de $2L$. Ce qui donne une différence de $L/2$ pour une rotation de 1 radian. Il s'agit là d'ailleurs de la définition du radian. Cette constante $L/2$ a été appelée MM_PER_RADIAN dans le code DSP.

Ainsi,
$x_n = x_{n-1} + \Delta x_n$
$y_n = y_{n-1} + \Delta y_n$
$\theta_n = \theta_{n-1} + \Delta \theta_n$

Or, l'orientation du robot doit toujours rester dans l'intervalle $[-\pi,+\pi]$. Ce choix est totalement arbitraire mais il nous est apparu comme étant le plus pertinent et le plus facile à utiliser; on aurait pu choisir $[0,2\pi]$. On a donc du écrire une fonction réalisant un modulo dont le résultat est toujours dans $[-\pi,+\pi]$, cette fonction nommée fmod_pmpi peut être trouvée dans math.c. C'est pourquoi on trouvera dans le code :
pos.angle = fmod_pmpi(pos.angle + (diff_d - diff_g)/MM_PER_RADIAN);

Le calcul et le stockage de la position sont désormais effectués sur des float. En effet, lors des premiers tests réalisés sur le portable, nous utilisions des float pour le calcul et des nombres entiers pour le stockage de la position en dixièmes de millimètres. Ce type de stockage engendrait une perte de précision non négligeable. C'est pourquoi nous réalisons maintenant tous les calculs en float ce qui apporte une souplesse d'utilisation importante et surtout ne prend pas plus de temps de calcul sur le DSP.

1.3.2 Approximation par des arcs de cercles

En reprenant les mêmes notations que précédemment, on obtient la figure suivante :

Figure 1.5: Approximation par des arcs de cercle

On a toujours :
$\Delta moy_n = \frac{\Delta d_n + \Delta g_n}{2}$
$\Delta dif_n = \Delta d_n - \Delta g_n$
$\Delta \theta_n = \frac{\Delta dif_n}{L}$

Si on tourne vers la droite ( $\Delta d_n>\Delta g_n$) :
$\Delta d_n = \Delta \theta_n \times (R+L/2)$
$\Delta g_n = \Delta \theta_n \times (R-L/2)$
par contre, si on tourne vers la gauche ( $\Delta d_n<\Delta g_n$) :
$\Delta d_n = \Delta \theta_n \times (R-L/2)$
$\Delta g_n = \Delta \theta_n \times (R+L/2)$

On en tire :
$R = L\frac{\Delta moy}{\Delta dif}$
$\Delta x_n = R [cos(\theta_{n-1}+\pi/2) + cos(\theta_{n}-\pi/2)]$
$\Delta y_n = R [sin(\theta_{n-1}+\pi/2) + sin(\theta_{n}-\pi/2)]$
ou encore,
$\Delta x_n = R [sin(\theta_{n}) - sin(\theta_{n-1})]$
$\Delta y_n = R [cos(\theta_{n-1}) + cos(\theta_{n})]$

Il faut bien voir qu'il est nécessaire de prévoir un cas spécial lorsque les deux roues parcourent exactement la même distance. Dans ce cas, l'arc de cercle se transforme en segment ce qui correspond à un cercle de rayon infini. Bien sûr, on ne peut pas faire de calcul avec une valeur infinie, c'est pourquoi, il faut prévoir un cas spécial.

Ce calcul doit absolument être effectué en virgule flottante (float) car il nécessite une dynamique très importante : rayon très grand et déplacements des roues très petits.

Or,
$\theta_{n} = \theta_{n-1} + \Delta \theta_n$

Ce qui donne après développement :
$\Delta x_n = R [sin(\theta_{n-1})cos(\Delta \theta_{n}) + cos(\theta_{n-1})sin(\Delta \theta_{n}) + sin(\theta_{n-1})]$
$\Delta y_n = R [sin(\theta_{n-1})sin(\Delta \theta_{n}) - cos(\theta_{n-1})cos(\Delta \theta_{n}) + cos(\theta_{n-1})]$

On retombe alors sur l'approximation par des segments de droites si on suppose que $\Delta \theta_{n}$ est très petit. En effet, dans ce cas :
$cos(\Delta \theta_{n}) \approx 1-\frac{\Delta \theta_{n}^2}{2}$
$sin(\Delta \theta_{n}) \approx \Delta \theta_{n}$

ce qui donne :
$\Delta x_n = R \left[cos(\theta_{n-1})\Delta \theta_{n} + sin(\theta_{n-1})\left(1 - \frac{\Delta \theta_{n}^2}{2} - 1\right) \right]$
$\Delta y_n = R \left[cos(\theta_{n-1})\left(1 - 1 + \frac{\Delta \theta_{n}^2}{2}\right) + sin(\theta_{n-1})\Delta \theta_{n} \right]$

ou encore :

$\Delta x_n = R \left[\Delta \theta_{n} cos(\theta_{n-1}) - \frac{\Delta \theta_{n}^2}{2}sin(\theta_{n-1}) \right]$
$\Delta y_n = R \left[\Delta \theta_{n} sin(\theta_{n-1}) + \frac{\Delta \theta_{n}^2}{2}cos(\theta_{n-1}) \right]$

Or, $\Delta \theta_n$ petit implique $\Delta \theta_n^2 \ll \Delta \theta_n^2$, d'où :

$\Delta x_n = R \Delta \theta_{n} cos(\theta_{n-1})$
$\Delta y_n = R \Delta \theta_{n} sin(\theta_{n-1})$

Mais, $R = L\frac{\Delta moy_n}{\Delta dif_n}$ et $\Delta \theta_n = \frac{\Delta dif_n}{L}$ donc $R \Delta \theta_n = \Delta moy_n$.

Ainsi,

$\Delta x_n = \Delta moy_{n} cos(\theta_{n-1})$
$\Delta y_n = \Delta moy_{n} sin(\theta_{n-1})$

L'approximation $\Delta \theta_{n}$ petit est d'autant plus justifiée que la fréquence d'échantillonnage $1/Te$ est très grande et que l'angle varie très peu entre deux estimations de la position ^1.8. Ceci explique donc que les deux méthodes d'approximation convergent lorsque l'on diminue la période d'échantillonnage.