Calculer la position du robot d'entraînement différentiel

Comment calculer ou mettre à jour la position d'un robot à entraînement différentiel avec des capteurs incrémentaux?

Un capteur incrémentiel est attaché à chacune des deux roues différentielles. Les deux capteurs déterminent la distance resp. leur roue a roulé pendant un temps connu .Δ r i g h t Δ $\Delta left$$\Delta right$$\Delta t$

Tout d'abord, supposons que le centre entre les deux roues marque la position du robot. Dans ce cas, on pourrait calculer la position comme:

$$x = \frac{x_{left}+x_{right}}{2} \\ y = \frac{y_{left}+y_{right}}{2}$$

«Dériver» ces équations en supposant que les deux roues roulaient en ligne droite (ce qui devrait être approximativement correct pour les petites distances), j'obtiens:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) \\ \frac{\Delta y}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)sin(\theta)$$

Où est l'angle d'orientation du robot. Pour le changement de cet angle, j'ai trouvé l'équation$\theta$

$$\frac{\Delta \theta}{\Delta t} = \frac{1}{w} \left( \frac{\Delta left}{\Delta t} - \frac{\Delta right}{\Delta t}\right)$$

Où $w$ est la distance entre les deux roues.

Étant donné que et dépendent de , je me demande si je devrais d'abord calculer la nouvelle en ajoutant ou si je devrais plutôt utiliser la "vieille" ? Y a-t-il une raison d'utiliser l'un sur l'autre?Δ y θ θ Δ θ $\Delta x$$\Delta y$$\theta$$\theta$$\Delta \theta$$\theta$

Supposons maintenant que le centre entre les deux roues ne marque pas la position du robot. Au lieu de cela, je veux utiliser un point qui marque le centre géométrique de la boîte englobante du robot. Ensuite, et deviennent:$x$$y$

$$x = \frac{x_{left}+x_{right}}{2} + l\, cos(\theta)\\ y = \frac{y_{left}+y_{right}}{2} + l\, sin(\theta)$$

"Dériver" le premier donne:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) - l\,sin(\theta)\,\frac{\Delta \theta}{\Delta t}$$

Il y a maintenant une dépendance sur . Est-ce une raison pour utiliser le "nouveau" ?$\Delta \theta$$\theta$

Existe-t-il une meilleure méthode pour effectuer une mise à jour simultanée de la position et de l'orientation? Peut-être utiliser des nombres complexes (même approche qu'avec des quaternions en 3D?) Ou des coordonnées homogènes?

Pour répondre à votre première question: si vous voulez vraiment trouver les vraies équations cinématiques pour l'entraînement différentiel, je ne commencerais pas à me rapprocher en supposant que chaque roue s'est déplacée en ligne droite. À la place, recherchez le rayon de braquage, calculez le point central de l'arc, puis calculez le point suivant du robot. Le rayon de braquage serait infini si le robot se déplace tout droit, mais dans le cas droit, les calculs sont simples.

Imaginez donc qu'à chaque pas de temps ou à chaque fois que vous calculez la variation des capteurs incrémentiels, le robot se déplace du point A au point B sur un arc comme celui-ci: Voici un exemple de code avec les mathématiques simplifiées:

// leftDelta and rightDelta = distance that the left and right wheel have moved along
//  the ground

if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
    new_x = x + leftDelta * cos(heading);
    new_y = y + rightDelta * sin(heading);
    new_heading = heading;
} else {
    float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
          wd = (rightDelta - leftDelta) / unitsAxisWidth;

    new_x = x + R * sin(wd + heading) - R * sin(heading);
    new_y = y - R * cos(wd + heading) + R * cos(heading);
    new_heading = boundAngle(heading + wd);
}

J'ai utilisé des mathématiques similaires dans un simulateur pour montrer différentes façons de diriger: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/

$\Delta\theta$$\theta$$\Delta{x}, \Delta{y}$

$\Delta\theta$$\Delta{x}, \Delta{y}$$\theta$

$\Delta{t}$$0$