Oblicz pozycję robota z napędem różnicowym

Jak obliczyć lub zaktualizować pozycję robota z napędem różnicowym za pomocą czujników inkrementalnych?

Do każdego z dwóch kół różnicowych dołączony jest jeden czujnik przyrostowy. Oba czujniki określają odległość lub. ich koło potoczyło się w znanym czasie .Δ R i g h t Δ $\Delta left$$\Delta right$$\Delta t$

Najpierw załóżmy, że środek między dwoma kołami oznacza pozycję robota. W takim przypadku można obliczyć pozycję jako:

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

„Wyprowadzając” te równania przy założeniu, że oba koła toczyły się w linii prostej (co powinno być w przybliżeniu prawidłowe dla małych odległości), otrzymuję:

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

Gdzie jest kątem orientacji robota. Dla zmiany tego kąta znalazłem równanie$\theta$

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

Gdzie to odległość między oboma kołami.$w$

Ponieważ i zależą od , zastanawiam się, czy powinienem najpierw obliczyć nowy , dodając czy raczej powinienem użyć „starej” ? Czy jest jakiś powód, aby używać jednego nad drugim?Δ y θ θ Δ θ $\Delta x$$\Delta y$$\theta$$\theta$$\Delta \theta$$\theta$

Załóżmy teraz, że środek między oboma kołami nie oznacza położenia robota. Zamiast tego chcę użyć punktu, który oznacza geometryczny środek obwiedni robota. Następnie i zmiany:$x$$y$

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

„Wyprowadzanie” pierwszego daje:

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

Teraz istnieje zależność od . Czy to jest powód, aby używać „nowego” ?$\Delta \theta$$\theta$

Czy jest jakaś lepsza metoda na jednoczesną aktualizację pozycji i orientacji? Może używać liczb zespolonych (takie samo podejście jak w przypadku czwartorzędów w 3D?) Lub współrzędnych jednorodnych?

Aby odpowiedzieć na pierwsze pytanie: jeśli naprawdę chcesz znaleźć prawdziwe równania kinematyczne dla napędu różnicowego, nie zacznę przybliżać, zakładając, że każde koło porusza się w linii prostej. Zamiast tego znajdź promień skrętu, oblicz punkt środkowy łuku, a następnie oblicz następny punkt robota. Promień skrętu byłby nieskończony, gdyby robot poruszał się prosto, ale w prostym przypadku matematyka jest prosta.

Więc wyobraź sobie, że na każdym etapie lub za każdym razem, gdy obliczasz zmianę czujników przyrostowych, robot przemieszcza się od punktu A do punktu B po łuku w następujący sposób: Oto przykładowy kod z uproszczoną matematyką:

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

Użyłem podobnej matematyki w symulatorze, aby zademonstrować różne sposoby sterowania: 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$