Calcola la posizione del robot di azionamento differenziale

Come si calcola o si aggiorna la posizione di un robot con azionamento differenziale con sensori incrementali?

C'è un sensore incrementale attaccato a ciascuna delle due ruote differenziali. Entrambi i sensori determinano la distanza risp. ha rotolato la ruota durante un tempo noto .Δ r i g h t Δ $\Delta left$$\Delta right$$\Delta t$

Innanzitutto, supponiamo che il centro tra entrambe le ruote indichi la posizione del robot. In questo caso, si potrebbe calcolare la posizione come:

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

"Derivando" quelle equazioni supponendo che entrambe le ruote ruotassero in linea retta (che dovrebbe essere approssimativamente corretta per le piccole distanze) ottengo:

$$\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)$$

Dove è l'angolo di orientamento del robot. Per il cambio di questo angolo ho trovato l'equazione$\theta$

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

Dove $w$ è la distanza tra entrambe le ruote.

Perché $\Delta x$ e $\Delta y$ dipendono $\theta$ , mi chiedo se devo prima calcolare la nuova $\theta$ aggiungendo $\Delta \theta$ o se devo invece utilizzare il "vecchio" $\theta$ ? C'è qualche motivo per usarne uno sopra l'altro?

Quindi, supponiamo ora che il centro tra le due ruote non segni la posizione del robot. Invece voglio usare un punto che segna il centro geometrico del rettangolo di selezione del robot. Poi $x$ ed $y$ modifica:

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

"Derivare" il primo dà:

$$\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}$$

Ora c'è una dipendenza da . È questo un motivo per usare il "nuovo" ?θ$\Delta \theta$$\theta$

Esiste un metodo migliore per eseguire l'aggiornamento simultaneo della posizione e dell'orientamento? Può usare numeri complessi (stesso approccio con i quaternioni in 3D?) O coordinate omogenee?

Per rispondere alla tua prima domanda: se vuoi davvero trovare le vere equazioni cinematiche per la trasmissione differenziale, non inizierei ad approssimare supponendo che ogni ruota si sia mossa in linea retta. Trova invece il raggio di sterzata, calcola il punto centrale dell'arco e quindi calcola il punto successivo del robot. Il raggio di sterzata sarebbe infinito se il robot si muovesse dritto, ma nel caso dritto la matematica è semplice.

Quindi, immagina che su ogni passaggio temporale o ogni volta che calcoli la variazione nei sensori incrementali, il robot viaggia dal punto A al punto B su un arco come questo: Ecco un codice di esempio con la matematica semplificata:

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

Ho usato una matematica simile in un simulatore per dimostrare diversi modi di guidare: 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$