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 )
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
/** @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;
}
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.