Menu

L'odométrie du robot principal de Sophia-Team 2004


Introduction

L'objectif de l'odométrie est de permettre au robot de connaitre sa position de la manière la plus précise. Pour cela on mesure le déplacement de roues sous le robot et par integration de cette information dans le temps, on en déduit la position du robot a chaque instant. Un inconvénient de cette méthode de localisation est que l'erreur sur la position augmente indéfiniment si le robot ne se recalle pas.
Dans le cadre de la coupe de France de robotique, les matchs durent 1m30 et l'erreur peut rester negligeable si les coefficients utilisés dans les formules de calcul de position sont bien calibrés et surtout si le robot ne glisse pas.

Système mécanique

Jusqu'en 2003 nous mesurions les déplacements du robot en mesurant la rotation des moteurs des roues motrices. L'avantage de cette méthode est que l'on peut mesurer d'infime déplacements du robot car la mesure se fait avant le système d'entrainement des roues motrice (donc avant réduction). Par contre dès que la vitesse de rotoation des roues varies, on subit le jeu des angrenages servant à la réduction. De plus il y a de forts risque de glissement des roues motrices, notamment si le robot freine brusquement. Il faut donc veiller à limiter l'accélération et la décélération des roues motrices pour éviter le glissement. Il reste un risque de glissement que l'on ne peut suppromer qui survient lors des collisions avec d'autres robots.
En 2004, en plus de la mesure des déplacements des roues motrices, nous avons utilisé des roues non motrices montées sur suspensions et alignées avec les roues motrices, pour mesurer les déplacements du robot. Ces roues sont en contact permanent avec le sol et ne glissent ni ne patinent quand le robot démarre ou s'arrète.
Il est important de prendre garde au système de suspension: les roues ne doivent avoir qu'un mouvement de translation vertical. Si les roues se déplacent dans le plan horizontal, les formules de calcul deviennent incorrètent.
Disposition des roues, principe

Disposition des roues, 3D

Disposition des roues, Photo

Système électronique

La mesure de la rotation se fait a l'aide de codeurs optiques HE........
Pour les roues motrices, ces codeurs sont directements relies à la carte qui régule la consigne envoyée aux moteurs (carte embarquée dans le PC équipée de HCTL1100). (Par la suite nous appellerons la mesure de position réalisée par ces capteurs: "HCTL").
Pour les roues non motrices, les codeurs sont interfacés avec une carte componsée d'un microcontrolleur Atmel et de 2 compteurs HCTL..... Le micro-controlleur récupère la position des roues toutes les 40ms et l'envoie au PC par l'intermédiaire de sa liaison série. (Par la suite nous appellerons la mesure de position réalisée par ces capteurs: "ODOM").

Architecture logicielle

Le thread (movementManager) chargé de gérer les déplacements du robot calcule periodiquement la position basée sur l'information des HCTL. En 204, la période de cette mesure était de l'ordre de 16ms.
Le thread (ioManager) chargé de récuppérer les informations des capteurs recoit toutes les 40ms la position des ODOM. Il alors à jour la position du robot utilisée par "movementManager".
La comparaison de ces deux mesures permet de déterminer si le robot est bloqué ou non.

Taches effectuées par le thread "movementManager":
  1. Calcul de la position basé sur les HCTL
  2. Comparaison position HCTL / position ODOM. Si les HCTL ont tournés beaucoup plus que les ODOM, le robot en déduit que ses roues tournent dans le vide. Il prévient le programme principal que le robot ne bouge pas.
  3. Si la consigne envoyée aux moteurs (PWM) sature pendant un certain temps, cela signifie aue les roues sont bloquées: le robot doit être contre un obstacle. Il prévient le programme principal que le robot ne bouge pas.
  4. Calcul de la consigne à envoyer aux moteurs en fonction du déplacement en cours.
  5. Envoie de la consigne aux moteurs

Formules de calcul de position

Formules d'odométrie
K: coefficient de conversion pas de codeur / millimètre
D: distance entre les 2 roues en millimètre
Theta_n: direction du robot à l'instant n
Theta_n-1: direction du robot à l'instant n-1
X_n: abscisse du robot à l'instant n
X_n-1: abscisse du robot à l'instant n-1
Y_n: ordonnée du robot à l'instant n
Y_n-1: ordonnée du robot à l'instant n-1
Cleft_n: Valeur du codeur gauche à l'instant n
Cleft_n-1: Valeur du codeur gauche à l'instant n-1
Cright_n: Valeur du codeur droit à l'instant n
Cright_n-1: Valeur du codeur droit à l'instant n-1

Theta_n = K / D x ( Cleft_n - Cright_n )
X_n = X_n-1 + K / 2 x ( Cleft_n - Cleft_n-1 + Cright_n - Cright_n-1 )
     x ( sin(Theta_n) - sin(Theta_n-1) ) / ( ( Theta_n + Theta_n-1 ) / 2 )
Y_n = Y_n-1 + K / 2 x ( Cleft_n - Cleft_n-1 + Cright_n - Cright_n-1 )
     x ( cos(Theta_n-1) - cos(Theta_n) ) / ( ( Theta_n + Theta_n-1 ) / 2 )

Les formules théoriques sont basées sur des robots parfaits. Cependant il est souvent nécessaire de tenir compte de certains facteurs d'erreurs et notamment la différence de diamètre entre les roues sur lesquelles sont montés les codeurs.

Kleft: coefficient de conversion pas de codeur / millimètre de la roue gauche
Kright: coefficient de conversion pas de codeur / millimètre de la roue droite

Theta_n = ( Kleft x Cleft_n - Kright x Cright_n ) / D
X_n = X_n-1 + ( Kleft x ( Cleft_n - Cleft_n-1) + Kright ( Cright_n - Cright_n-1 ) )
     x ( sin(Theta_n) - sin(Theta_n-1) ) / ( Theta_n + Theta_n-1 )
Y_n = Y_n-1 + ( Kleft x ( Cleft_n - Cleft_n-1) + Kright ( Cright_n - Cright_n-1 ) )
     x ( cos(Theta_n-1) - cos(Theta_n) ) / ( Theta_n + Theta_n-1 )

Ed: rapport entre le diamètre de la roue gauche et la roue droite
Kleft = K x 2 / ( Ed +1 )
Kright = K x 2 / ( 1 / Ed +1 )


Code de calcul de position

Voir la classe RobotPosition pour le calcul de position
src/include/robotPosition.h
src/move/robotPosition.cpp
src/motor/motor.cpp : Analyse des valeurs des codeurs des moteurs
src/io/odometer/odometer_04.cpp : Reccupération des données des codeurs des roues libres sur liaison série

/** @brief Conversion pas de codeur ODOMETRE/distance parcourue par une roue odometre */
static const Millimeter POSITION_ROBOT_ODOM_K = .057521639;
/** @brief Distance entre les 2 roues motrices */
static const Millimeter POSITION_ROBOT_ODOM_D = 206.11;
/** @brief Différence de diamètre entre les roues */
static const double POSITION_ROBOT_ODOM_Ed = 0.9972640;
/** @brief Coefficient pour la roue droite */
static const double POSITION_ROBOT_ODOM_Cr = 2./((1./POSITION_ROBOT_ODOM_Ed)+1.);
/** @brief Coefficient pour la roue gauche */
static const double POSITION_ROBOT_ODOM_Cl = 2./(POSITION_ROBOT_ODOM_Ed+1.);
/** @brief Signe du codeur hctl gauche en allant vers l'avant */
static const double POSITION_CODER_ODOM_SIGN_LEFT = 1.;
/** @brief Signe du codeur hctl droit en allant vers l'avant */
static const double POSITION_CODER_ODOM_SIGN_RIGHT = -1.;

/** @brief Unité de distance par défaut */
typedef double Millimeter;
/** @brief Unité d'angle par défaut */
typedef double Radian;
/** @brief Unite des codeurs montes sur des axes de rotation (odometres) */
typedef int CoderPosition;
/** @brief Point pour le robot, on parle en nombre a virgules */
typedef RobotPoint2D Point;
/** @brief Une position est un point et une direction */
typedef struct Position {
  Point center;
  Radian direction;
  // constructeur
  Position(): center(), direction(0){}
  Position(Point pt, Radian t): center(pt), direction(t){}
  Position(Millimeter x, Millimeter y, Radian t):
    center(Point(x,y)), direction(t){}
} Position;

// ----------------------------------------------------------------------------
// RobotPosition::getPosition
// ----------------------------------------------------------------------------
void RobotPosition::getPosition(
  Position& posi, // Position qui va être mise à jour
  CoderPosition leftPos, // Valeur codeur gauche
  CoderPosition rightPos,// Valeur codeur droit
  CoderPosition& leftPosOld, // Ancienne valeur codeur gauche
  CoderPosition& rightPosOld, // Ancienne valeur codeur droit
  Millimeter KLeft, // Coeficient K pour la roue gauche
  Millimeter KRight, // Coeficient K pour la roue droite
  Millimeter D)// Distance entre les roues
{
  // verifie qu'il n'y a pas un passage de -MAX a +MAX (overflow)
  if (leftPos - leftPosOld > SHRT_MAX) { // short max =unsigned short max /2
    leftPosOld += USHRT_MAX; // unsigned short max
  } else if (leftPos - leftPosOld < SHRT_MIN) {
    leftPosOld -= USHRT_MAX;
  }
  if (rightPos - rightPosOld > SHRT_MAX) {
    rightPosOld += USHRT_MAX;
  } else if (rightPos - rightPosOld < SHRT_MIN) {
    rightPosOld -= USHRT_MAX;
  }

  Millimeter deltaKRight = KRight * (rightPos - rightPosOld);
  Millimeter deltaKLeft = KLeft * (leftPos - leftPosOld);
  rightPosOld = rightPos;
  leftPosOld = leftPos;

  // Calcul de la direction du robot
  Radian oldTheta = posi.direction;
  Radian deltaTheta = Radian((deltaKRight - deltaKLeft) / D);
  posi.direction += deltaTheta;

  // Calcul de la position du robot
  Millimeter deltaSum = (deltaKRight + deltaKLeft) / 2.;
  if (fabs(deltaTheta) > 0.0001) {
    // Le robot tourne
    posi.center.x += (Millimeter)( deltaSum * (sin(posi.direction) - sin(oldTheta)) / deltaTheta);
    posi.center.y += (Millimeter)(-deltaSum * (cos(posi.direction) - cos(oldTheta)) / deltaTheta);
  } else {
    // Le robot va tout droit
    deltaTheta = (posi.direction + oldTheta) / 2.;
    posi.center.x += deltaSum * cos(deltaTheta);
    posi.center.y += deltaSum * sin(deltaTheta);
  }
}

// ----------------------------------------------------------------------------
// RobotPosition::updateOdometerPosition
// ----------------------------------------------------------------------------
void RobotPosition::updateOdometerPosition()
{
  CoderPosition left=0, right=0;
  if (!ODOMETER->getCoderPosition(left, right)) {
    return;
  }
  Position oldPos=posOdom_;
  getPosition(
    posOdom_,
    left,
    right,
    leftOdomOld_,
    rightOdomOld_,
    POSITION_CODER_ODOM_SIGN_LEFT * POSITION_ROBOT_ODOM_K * POSITION_ROBOT_ODOM_Cl,
    POSITION_CODER_ODOM_SIGN_RIGHT * POSITION_ROBOT_ODOM_K * POSITION_ROBOT_ODOM_Cr,
    POSITION_ROBOT_ODOM_D,
    firstOdom_);
  firstOdom_ = false;
}


Méthode de calibration des coefficients

Il y a 3 coefficients à calibrer.

En premier, on détermine la distance parcourrue par le robot quand le codeur tourne d'un pas: K
Il suffit d'envoyer le robot en ligne droite. D'après les formules précédentes, on a: Destimee = K x NombrePasDuCodeur En première approche on estime à partir des données constructeurs et du rapport de réduction K. Puis on fait se déplacer le robot en ligne droite sur plusieurs mètres et on en déduit une nouvelle valeur de K:
Knew = Kold x DistRéelle / DistEstimée

En second, on détermine la distance entre les deux roues codeuses: D
En première approximation on mesure la distance entre les deux roues avec une règles. Puis on fait tourner le robot sur lui même sur plusieurs tours et on en déduit une nouvelle valeur de D:
Bnew = Bold x DirEstimée / DirRéelle

En troisième, on détermine la différence de diamètre entre les deux roues codeuses: Ed
En première approximation Eb=1 ce qui signifie que les roues ont le même diamètre. Puis on fait avancer le robot en ligne droite. Si le robot est plus à gauche qu'il ne le croit, c'est que la roue gauche est plus grande que la roue droite. Dans ce cas il faut augmenter Ed. Attention une différence de 0.3% sur le diamètre des roues peut entrainer une déviation de 2cm après seulement 2m.

Il faut ensuite itérer plusieurs fois entre les étapes 2 et 3 pour arriver à une bonne estimation de ces coefficients.

Références