Рассчитать положение дифференциального привода робота

Как рассчитать или обновить положение робота с дифференциальным приводом с помощью инкрементальных датчиков?

К каждому из двух колес дифференциала подключен один инкрементальный датчик. Оба датчика определяют расстояние соответственно. Δ r i g h t их колесо катилось в течение известного времени Δ t .$\Delta left$$\Delta right$$\Delta t$

Во-первых, давайте предположим, что центр между двумя колесами отмечает положение робота. В этом случае можно рассчитать позицию как:

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

«Выводя» эти уравнения в предположении, что оба колеса катятся по прямой (что должно быть приблизительно правильно для небольших расстояний), я получаю:

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

Где - угол ориентации робота. Для изменения этого угла я нашел уравнение$\theta$

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

Где - расстояние между обоими колесами.$w$

$\Delta x$$\Delta y$$\theta$$\theta$$\Delta \theta$$\theta$

$x$$y$

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

«Вывод» первый дает:

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

$\Delta \theta$$\theta$ ?

Есть ли лучший способ сделать одновременное обновление позиции и ориентации? Может быть, использовать комплексные числа (такой же подход, как с кватернионами в 3D?) Или однородные координаты?

Чтобы ответить на ваш первый вопрос: если вы действительно хотите найти истинные кинематические уравнения для дифференциального привода, я бы не стал приближаться, предполагая, что каждое колесо движется по прямой. Вместо этого найдите радиус поворота, рассчитайте центральную точку дуги, а затем вычислите следующую точку робота. Радиус поворота будет бесконечным, если робот движется прямо, но в прямом случае математика проста.

Итак, просто представьте, что на каждом временном шаге или каждый раз, когда вы вычисляете изменение в инкрементальных датчиках, робот перемещается из точки A в точку B по дуге, как это: Вот пример кода с упрощенной математикой:

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

Я использовал подобную математику в симуляторе, чтобы продемонстрировать различные способы управления: 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$