Навигация по точкам GPS для робота, работающего на улице

Градусы — неподходящая единица для робота

Модуль GPS выдаёт широту и долготу в градусах. Каждый ваш контроллер ожидает метры. Преобразование — первая задача, и она проще, чем кажется из-за интернета.

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

north (m) = (lat − lat₀) × 111 320
east  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

Один раз выберите начало координат lat₀, lon₀ — в начале миссии — и каждая точка маршрута станет обычной парой (east, north) в метрах, по которой метод pure pursuit сможет рулить, ничего не зная о градусах.

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

Число, которое стоит запомнить: один градус широты равен 111.3 km, поэтому пять знаков после запятой дают 1.1 m, а шесть — 11 cm. Храните точки маршрута с шестью знаками. Больше — это хранение шума, а float на AVR содержит всего около семи значащих цифр: достаточно для 19.076123 и недостаточно для большего, поэтому переводите значения в метры относительно начала координат, а не выполняйте арифметику с исходными градусами.

Три проблемы показаний GPS

Оно запаздывает. Обычный NEO-6M выдаёт данные с частотой 1 Hz. При 1.5 m/s между обновлениями робот проезжает 1.5 m, а полученное показание описывает место, где робот находился при вычислении решения, а не там, где он находится сейчас.

Оно шумное. Бытовой GPS на открытом месте с ясным небом примерно в половине случаев находится в пределах 2.5 m от истинного значения. Под деревьями, рядом со зданием или при плохом расположении спутников всё значительно хуже — причём это не обнаруживаемый пропуск, а правдоподобный неправильный ответ.

Оно не содержит курса. Показание GPS задаёт положение. Оно не может сообщить, куда направлен робот, потому что неподвижный робот, направленный на север, и тот же робот, направленный на юг, дают одинаковые показания. Путевой курс вычисляется по последовательным положениям и имеет смысл только при действительном движении — примерно ниже 1 m/s он в основном является шумом.

Что это делает со слежением за маршрутом

Передайте одному и тому же регулятору pure pursuit один и тот же курс дважды: один раз с идеальной одометрией, другой — с обновлением GPS 1 Hz, удерживаемым между обновлениями: ±1.5 m.

Обратите внимание, чего не происходит: робот не теряется. Он примерно держится курса и достигает конца. Навигация только по GPS работает — она просто намного хуже, чем кажется по техническому описанию, и основная цена ложится на исполнительные механизмы.

Три исправления в порядке ценности

1 — Объединяйте данные, а не используйте GPS напрямую. Интегрируйте одометрию колёс и источник курса с частотой контура, а каждое показание GPS используйте для исправления оценки, а не для замены. Даже простое комплементарное объединение заметно улучшает результат:

// Between fixes: dead reckon. This runs at the loop rate.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// On a fix: pull the estimate toward it, gently. alpha near 0.1 trusts odometry
// over the second between fixes, which is exactly the right instinct.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

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

2 — Получайте курс от другого источника. Магнитометр даёт абсолютный курс на месте, но требует калибровки жёсткого и мягкого железа и расстояния до двигателей. Гироскоп даёт отличный краткосрочный курс и дрейфует. Вместе их использует каждый настоящий ровер; путевой курс по GPS затем корректирует эту пару, когда движение достаточно быстрое, чтобы ему доверять.

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

Чтение модуля

Модули GPS обмениваются данными по последовательному порту со скоростью 9600 baud в формате NMEA 0183: сообщения с запятыми, по одному на строку. Нужны два из них.

• $GPRMC — время, достоверность, широта, долгота, скорость по земле и путевой курс.
• $GPGGA — качество решения, число использованных спутников и HDOP — горизонтальный коэффициент снижения точности.

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

• Отбрасывайте решение со статусом V (void) или с качеством решения 0. Оно всё равно содержит правдоподобные на вид числа.
• Отбрасывайте HDOP выше примерно 2.5. HDOP описывает геометрию спутников, и плохая геометрия даёт уверенный ответ с ошибкой в десятки метров.

Используйте библиотеку разбора, а не пишите собственную, но прочитайте достаточно предложений, чтобы понимать, что вы отбрасываете. Кроме того: холодный запуск без альманаха занимает от 30 секунд до нескольких минут. Не считайте робота без решения роботом в начале координат. Ждите и покажите видимую индикацию ожидания.

Задайте геозону до испытаний

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

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // before anything else

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

Когда всё идёт не так

Проведите один и тот же маршрут с обоими источниками положения в симуляторе следования по пути, затем соберите робота для навигации по точкам GPS — там сам модуль и описанное выше объединение составляют весь проект.