Одометрия дифференциального привода

Как робот на двух колёсах понимает, куда он приехал, просто считая обороты колёс.

Одометрия — оценка перемещения робота по данным с датчиков движения (энкодеров колёс) путём интегрирования скоростей.

У дифференциального привода два ведущих колеса с независимыми скоростями $v_L, v_R$ и расстоянием между ними $b$ (база). Из них получаются линейная и угловая скорости центра робота:

$$ v = \frac{v_R + v_L}{2}, \qquad \omega = \frac{v_R - v_L}{b} $$

Если колёса крутятся одинаково — едем прямо ($\omega = 0$); если по-разному — поворачиваем. Чтобы развернуться на месте, колёса крутят в противоположные стороны.

Интегрирование позы

Зная $v, \omega$, обновляем позу $(x, y, \theta)$ за малый шаг $\Delta t$:

$$ \theta \mathrel{+}= \omega \, \Delta t, \quad x \mathrel{+}= v \cos\theta \, \Delta t, \quad y \mathrel{+}= v \sin\theta \, \Delta t $$

Считаем

$v_L = 0.5$, $v_R = 0.7$ м/с, база $b = 0.3$ м. Проедем 1 секунду шагами по 0.1 с.

import math

vL, vR, b = 0.5, 0.7, 0.3
v = (vR + vL) / 2
w = (vR - vL) / b
print("v =", round(v, 4), "м/с,  w =", round(w, 4), "рад/с")

x = y = theta = 0.0
dt = 0.1
for _ in range(10):
    theta += w * dt
    x += v * math.cos(theta) * dt
    y += v * math.sin(theta) * dt

print("через 1 с: x =", round(x, 4), "y =", round(y, 4),
      "theta =", round(math.degrees(theta), 2), "град")

Вывод:

v = 0.6 м/с,  w = 0.6667 рад/с
через 1 с: x = 0.5499 y = 0.2112 theta = 38.2 град

Как работает под капотом

Энкодеры считают «тики» — доли оборота колеса. Умножив на длину окружности колеса, получаем пройденный каждым колесом путь, отсюда $v_L, v_R$, отсюда — приращение позы. Это счисление пути (dead reckoning): дёшево и быстро, но ошибки накапливаются — проскальзывание колёс и неточность базы со временем уводят оценку от истины. Поэтому одометрию дополняют другими датчиками (IMU, лидар) и фильтрами.

Частые ошибки

• Обновлять $x, y$ по старому $\theta$, а не по обновлённому — для маленького $\Delta t$ разница мала, но при больших шагах накапливается.
• Путать базу $b$ (между колёсами) с радиусом колеса — это разные величины.
• Доверять одометрии на длинной дистанции без коррекции — дрейф неизбежен.

Итог

• Дифференциальный привод задаёт $v$ и $\omega$ через скорости двух колёс.
• Поза интегрируется пошагово из $v, \omega$ и $\Delta t$.
• Одометрия — дешёвое счисление пути, но ошибки накапливаются (дрейф).
• Для точности её комбинируют с другими датчиками.