TP4 : Odométrie¶

Le but de la séance est de découvrir les principes de l'odométrie.

Exercice 1 : Modèle direct¶

Le but de l'exercice est de faire parcourir à votre robot une trajectoire circulaire dont on choisit le rayon et à une vitesse déterminée.

Quelques notations :

\(R\) le rayon de la trajectoire circulaire en mètres
\(s\) la vitesse choisie en mètres par secondes
\(dw\) la distance entre les roues en mètres
\(R_w\) le rayon des roues en mètres

Si le parcours se fait dans le sens trigonométrique, la roue gauche doit parcourir un cercle de rayon \(R-dw/2\) et la roue droite \(R+dw/2\).

Pour convertir une vitesse en vitesse angulaire, il faut diviser par le rayon. La vitesse angulaire du robot sur son cercle est donc de \(s / R\).

Par conséquent, la vitesse de la roue gauche sur sa trajectoire sera de \((R-dw/2)\times(s/R)\) et celle de la roue droite \((R+dw/2)\times(s/R)\).

Finalement, la vistesse angulaire des roue gauche et droite seront données par :

\[ \left\{ \begin{aligned} \omega_l & = \frac{(R-\frac{dw}{2})\times(\frac{s}{R})}{R_w}\\ \omega_r & = \frac{(R+\frac{dw}{2})\times(\frac{s}{R})}{R_w} \end{aligned} \right. \]

Attention, ces valeurs sont en radians par secondes... Pour convertir des radians en degrés (unité du paramètre de Motor.setSpeed() est degrés par secondes) vous pouvez utiliser la méthode Math.toDegrees().

Faites un programmes tout simple en complétant le squelette :

import lejos.nxt.*;

public class DriveCircle {

    public static final Motor LM = Motor.A;
    public static final Motor RM = Motor.B;


    public static final double RADIUS       = 126.0/1000;   // 12,6cm
    public static final double SPEED        = 5.0/100;  // 5cm/sec
    public static final double W_GAP        = 14.0/100; // 14cm 
    public static final double W_RADIUS     = 2.0/100;  // 2cm 

    public static void main (String[] args) {

        System.out.println("Drive Circle");
        Button.waitForPress();

        double lws = ...
        double rws = ... 

        int lwsd = (int)Math.toDegrees(lws); 
        int rwsd = (int)Math.toDegrees(rws); 

        LM.setSpeed(lwsd ); // degree/sec
        RM.setSpeed(rwsd );

        LM.forward();
        RM.forward();

        Button.waitForPress();

    }
}

Testez... Ca marche ? Votre robot a combien de roue (une chenille n'est pas une roue) ? Modifiez si besoin votre maquette.

Exercice 2 : Modèle indirect¶

On veut maintenant calculer les positions du robot, c'est à dire faire l'inverse de précédemment. À partir de la donnée déplacement angulaire des roues, on veut estimer le changement de position du robot.

L'astuce consiste à supposer que sur un temps suffisemment court, tout déplacement du robot peut s'apparenter à un cercle de rayon \(R\) et de centre \((x_0,y_0)\). À partir du modèle direct, on peut alors extraire le modèle indirect soit les équations :

\[ \left\{ \begin{aligned} s = & \frac{s_l + s_r}{2} \\ R = & \frac{dw}{2} \times \frac{s_r+s_l}{s_r-s_l} \end{aligned} \right. \]

Remarquez que l'on peut tout à fait utiliser des distances au lieu des vitesses, une vitesse étant une distance parcourue sur un temps donné, on obtient les équations (le temps n'apparait plus) :

\[ \left\{ \begin{aligned} d = & \frac{d_l + d_r}{2} \\ R = & \frac{dw}{2} \times \frac{d_r+d_l}{d_r-d_l} \end{aligned} \right. \]

Nous savons donc déjà calculer \(R\), nous allons voir comment récupérer \((x_0,y_0)\), et donc comment on peut calculer la nouvelle position du robot \((x',y',\theta')\) connaissant l'ancienne \((x,y,\theta)\).

Notons avant cela que les entrées sont désormais les distances parcourures par les deux roues. Ces distances peuvent être retrouvées grace à la méthode de Motor getTachoCount() qui renvoit l'angle en degré parcouru. En passant cet angle en radians, puis en le multipliant par le rayon de la roue, on obtient la distance parcourue par la roue.

Si on connait \((x,y,\theta)\) la position du robot à un instant donné puis le déplacement des roues \(d_l\) et \(d_r\) depuis cet instant on a alors avec un peu de trigonométrie :

\[ \left\{ \begin{aligned} d\theta & = \frac{\frac{d_l + d_r}{2}}{R} \\ x_0 & = x - R \times cos(\theta - \frac{\pi}{2}) \\ y_0 & = y - R \times sin(\theta - \frac{\pi}{2}) \end{aligned} \right. \]

où \((x_0, y_0)\) sont donc les coordonnées du centre du cercle décrit par le robot pendant l'intervalle depuis la dernière mesure et \(d\theta\) l'angle parcouru sur ce cercle.

On peut alors calculer les nouvelles coordonnées du robot en refaisant un peu de trigonométrie :

\[ \left\{ \begin{aligned} \theta' & = \theta + d\theta \\ x' & = x_0 +R \times cos(\theta' - \frac{\pi}{2}) \\ y' & = y_0 +R \times sin(\theta' - \frac{\pi}{2}) \end{aligned} \right. \]

Ajoutez au programme précédant une boucle qui calcule les positions du robot dans une boucle et un thread qui les affiche toute les secondes.

À chaque tour de boucle vous devez commencer par récupérer l'angle (avec getTachoCount()) de chaque roue et mesurer le delta depuis la mesure précédante. Ensuite les autres valeurs sont obtenues grâce aux équations.

Attention

En java, lorsqu'un double est divisé par 0, sa valeur devient infinity et si vous faites une opération entre infinity et un double, le résultat sera NaN (Not a Number). Chaque fois que ce risque existe, vous pouvez tester le résultat avec un code comme :

double d = ... ;
if(Double.isInfinite(d) || Double.isNaN(d)) { 
    // cas particulier
}else{
    // cas normal
}

Question : quand le robot avance en ligne droite, comment peut on écrire plus simplement les équations ?

\[ \left\{ \begin{aligned} \theta' & = f^?(\theta) \\ x' & = x + f^?(d) \\ y' & = y + f^?(d) \end{aligned} \right. \]

Ajoutez au programme du TP3 une classe RTOdometry qui représente un thread temps réel qui estime la position du robot par odométrie toutes les 50ms (période à adapter empiriquement ...)
Faites en sorte que le Thread RTOdometry arrête le robot après un tour (lorsque les coordonnées redeviennent proches de \((0,0,0)\)).