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

У робота с дифференциальным приводом два независимо приводимых колеса, разделённых шириной колеи. Когда оба колеса проходят одинаковое расстояние, робот движется прямо. Когда одно проходит дальше, шасси движется по дуге. Одометрия интегрирует эти малые перемещения во времени, чтобы оценить позу: координаты x, y и курс θ.

Оценка локальная и накопительная. Она начинается с выбранной позы и не может исправить себя только по энкодерам колёс. Небольшие ошибки диаметра колёс, ширины колеи, времени и проскальзывания накапливаются, поэтому цель — полезная краткосрочная оценка с измеренными пределами, а не идеальная истинная координата.

Задайте одно соглашение о координатах

Выберите систему до написания уравнений. Распространённое двумерное соглашение:

• x направлена вперёд от начальной позы.
• y направлена влево.
• положительный θ — поворот против часовой стрелки.
• расстояния левого и правого колёс положительны при движении робота вперёд.

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

Пусть dL и dR — расстояния левого и правого колёс с предыдущего обновления, а b — эффективное расстояние между линиями контакта колёс.

Превратите отсчёты в метры

Всё выше предполагает, что dL и dR уже измерены в метрах. Энкодеры выдают не метры, а отсчёты. Между ними есть одно преобразование, и ошибка в нём масштабирует каждое число, которое вычисляет остальная страница.

countsPerWheelTurn = PPR x edgesDecoded x gearRatio
metresPerCount     = (pi x wheelDiameter) / countsPerWheelTurn

edgesDecoded равен 4, только если декодируются оба фронта обоих каналов — полная квадратура. Декодирование только нарастающего фронта одного канала даёт 1 и заодно выбрасывает информацию о направлении, поэтому робот не может отличить движение назад от движения вперёд.

Нижняя строка удивляет людей. Щелевой диск, стянутый на колесе нейлоновой стяжкой, дёшев, но квантование 10 mm означает, что перемещение 200 mm известно примерно на 5%, и никакая калибровка не восстановит информацию, которую датчик никогда не захватывал. Учебник по квадратурным энкодерам полностью раскрывает декодирование.

Разрешение — нижняя граница точности, а не обещание точности. Отсчёт 73 um бессмысленен, если диаметр колеса ошибочен на 2%.

Вычислите поступательное и угловое движение

Центр оси перемещается на среднее расстояние колёс:

dCenter = (dR + dL) / 2

Курс изменяется в соответствии с разностью:

dTheta = (dR - dL) / b

Одинаковые расстояния дают dTheta = 0. Равные и противоположные расстояния дают dCenter = 0, поэтому робот вращается на месте вокруг середины оси.

Для небольших обновлений применение перемещения в промежуточном курсе точно и позволяет не делать отдельный случай для прямой:

headingMid = theta + dTheta / 2
x = x + dCenter * cos(headingMid)
y = y + dCenter * sin(headingMid)
theta = theta + dTheta

Такое интегрирование в середине достаточно просто для встраиваемого контроллера и хорошо ведёт себя при частом цикле. Точная интеграция дуги полезна при больших шагах, но не исправляет неправильные измерения колёс.

Реализуйте повторяемое обновление

Храните исходные суммы энкодеров и переводите их разности в метры. Не сбрасывайте аппаратные счётчики на каждом цикле; фиксация показаний упрощает обработку переполнения и диагностику.

struct Pose { float x, y, theta; };

void updateOdometry(Pose& pose, float dLeft, float dRight, float trackWidth) {
  const float dCenter = 0.5f * (dLeft + dRight);
  const float dTheta = (dRight - dLeft) / trackWidth;
  const float headingMid = pose.theta + 0.5f * dTheta;

  pose.x += dCenter * cosf(headingMid);
  pose.y += dCenter * sinf(headingMid);
  pose.theta += dTheta;
}

Вызывайте это после атомарного чтения обоих энкодеров. Показания левого и правого энкодеров должны соответствовать почти одному моменту; большой промежуток между ними создаёт кажущееся руление при быстром движении.

Если другой подсистеме нужна скорость, разделите приращения на измеренное прошедшее время. Сама интеграция позы использует расстояние, поэтому дрожание времени цикла напрямую не меняет оценку пройденного расстояния.

Чтение обоих энкодеров без потери отсчётов

Функция выше — чистая арифметика. Ошибки встраиваемой одометрии чаще возникают при подготовке входных данных, и почти весь риск приходится на три детали.

Считывайте оба счётчика в один момент. Обработчики прерываний продолжают увеличивать их, пока цикл читает значения. Если прочитать левый, выполнить работу, затем прочитать правый, движение между этими моментами припишется только одному колесу — уравнения верно интерпретируют это как поворот.

Никогда не сбрасывайте аппаратные счётчики. Непрерывные суммы и запомненное предыдущее значение дают разность, а исходный отсчёт остаётся доступным для диагностики.

Вычисляйте разность в беззнаковой арифметике. Переполнение знакового типа в C++ имеет неопределённое поведение; приведение ниже определено, ничего не стоит и остаётся корректным при переполнении.

volatile int32_t leftCount = 0, rightCount = 0;   // written only by the ISRs

void isrLeft()  { leftCount  += digitalRead(L_CHAN_B) ? +1 : -1; }
void isrRight() { rightCount += digitalRead(R_CHAN_B) ? +1 : -1; }

constexpr float METRES_PER_COUNT = (3.14159265f * 0.065f) / 2800.0f;   // 72.9 um
constexpr float TRACK_WIDTH      = 0.152f;                             // calibrated, not measured

void odometryTick(Pose& pose) {
  int32_t nowLeft, nowRight;
  noInterrupts();                      // one instant, both wheels
  nowLeft  = leftCount;
  nowRight = rightCount;
  interrupts();

  static int32_t prevLeft = 0, prevRight = 0;
  // Unsigned subtraction wraps in a defined way; the cast back recovers the
  // signed delta even across the 2^32 boundary.
  const int32_t dLeftCounts  = (int32_t)((uint32_t)nowLeft  - (uint32_t)prevLeft);
  const int32_t dRightCounts = (int32_t)((uint32_t)nowRight - (uint32_t)prevRight);
  prevLeft  = nowLeft;
  prevRight = nowRight;

  updateOdometry(pose,
                 dLeftCounts  * METRES_PER_COUNT,
                 dRightCounts * METRES_PER_COUNT,
                 TRACK_WIDTH);
}

При 73 um на отсчёт знаковый 32-битный счётчик переполняется примерно через 157 km, поэтому для настольного робота это теоретическая проблема. Всё равно потратим два приведения, чтобы сделать это правильно; тот же приём обязателен для 16-битного счётчика, который переполняется через 2.4 m.

Разберите один поворот

Пусть левое колесо проходит 0.20 m, правое — 0.30 m, а ширина колеи равна 0.25 m:

• dCenter = 0.25 m
• dTheta = (0.30 - 0.20) / 0.25 = 0.4 rad
• при начальном нулевом курсе промежуточный курс равен 0.2 rad
• dx ≈ 0.25 cos(0.2) = 0.245 m
• dy ≈ 0.25 sin(0.2) = 0.050 m

Робот заканчивает немного левее исходной оси направления и поворачивается примерно на 22.9°. Такая ручная проверка обнаруживает ошибки знаков и единиц до того, как код дойдёт до оборудования.

Откалибруйте два главных параметра

Эффективный диаметр колеса управляет масштабом расстояния. Проедьте длинную измеренную прямую, сравните сообщённое и действительное расстояние и умножьте масштаб диаметра на actual / reported. Если робот заворачивает несмотря на одинаковые команды, откалибруйте каждую сторону отдельно.

Эффективная ширина колеи управляет масштабом вращения. Задайте несколько полных вращений на месте, измерьте действительное изменение курса внешним средством и отрегулируйте ширину колеи. Несколько оборотов увеличивают ошибку и облегчают измерение. Из-за проскальзывания шин эффективное значение может отличаться от измеренной линейкой ширины между колёсами.

Оставьте оба параметра настраиваемыми. Поверхность, сжатие шин, полезная нагрузка и износ могут их менять; аппаратные модели нуждаются в параметрах калибровки.

Знайте, как быстро растёт ошибка

Калибровка не устраняет ошибку, а уменьшает коэффициент. Зная размер оставшейся ошибки, вы понимаете, сколько можно доверять оценке до того, как что-то внешнее должно её исправить.

Значение для квадрата получено интегрированием квадрата 1 m с четырьмя поворотами на месте по 90 deg и единственной ошибкой ширины колеи. Всё остальное точно, а робот всё равно заканчивает на 8.7 cm от начальной точки.

Сопоставьте последнюю строку с первыми двумя. Математика интегрирования никогда не является главным источником ошибки. Она на три порядка ниже ошибок калибровки и на четыре ниже проскальзывания, поэтому повышать частоту цикла ради точности — неправильная оптимизация; и именно поэтому страница о проскальзывании колёс важнее итоговой позы, чем всё, что есть на этой странице.

Также отметим, что систематические и случайные ошибки ведут себя совершенно по-разному. Систематическое проскальзывание накапливается линейно, и его стоит устранить калибровкой. Случайное растёт пропорционально квадратному корню числа поворотов, поэтому 50 поворотов стоят около 6 deg, а не 45: это раздражает, но не исправляется настройкой параметров.

Выберите частоту обновления

Дуга между двумя измерениями аппроксимируется прямой хордой с промежуточным курсом, поэтому ошибка зависит от изменения курса за шаг. Для шага dTheta радиан хорда коротка примерно на dTheta^2 / 24:

Выберите 50 Hz и перестаньте думать об этом. Сэкономленное внимание потратьте на синхронизацию двух чтений энкодеров: при скорости 0.5 m/s разница во времени чтения 1 ms между левым и правым снимком на каждом цикле выдумывает 0.5 mm разницы колёс из ничего, что важнее самой частоты.

Добавьте гироскоп, не путая системы координат

Гироскоп часто даёт лучшее краткосрочное изменение курса, чем разность энкодеров колёс. Можно использовать курс гироскопа для theta, сохранив среднее расстояние колёс для перемещения. Применяйте перемещение в середине старого и нового курсов гироскопа.

Это не делает позицию абсолютной. У гироскопа есть смещение, а пройденное колёсами расстояние по-прежнему искажается проскальзыванием. Для исправления накопленной позы нужен глобальный датчик — ориентиры камеры, локализация по лидару или другой внешний источник.

В терминологии ROS непрерывная локальная оценка естественно принадлежит системе odom. Система локализации может обновлять map → odom, не заставляя локально плавное преобразование odom → base_link скакать.

Измеряйте дрейф, а не прячьте его

Выполните три повторяемых теста:

1. Проедьте вперёд и назад до начальной точки.
2. Выполните несколько оборотов и вернитесь к исходному курсу.
3. Проедьте большой квадрат и сравните конечную позу с начальной отметкой.

Записывайте разности энкодеров, вычисленную позу, напряжение аккумулятора и поверхность. Повторяйте каждый тест в обоих направлениях. Постоянная ошибка масштаба указывает на калибровку; ошибка, меняющаяся с направлением, — на люфт или асимметрию; ошибка, меняющаяся от поверхности, — на проскальзывание.

Диагностика

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