Raspberry-pi-танк и GPS навигация
После экспериментов с искуственным интеллектом автопилот моего танка решил сделать шаг назад и научиться ориентироваться по старым добрым спутникам.
Картинка постановочная, намекающая, что всякий GPS транспорт умеет самостоятельно топиться под управлением навигатора.
Raspberry Pi + GPS
Сначала был закуплен простой USB-модуль GPS.
Руководствуясь статьей на Адафрукте, я приступил к внедрению.
Несмотря на то, что статья написана 6 лет назад, в мире олдскульной навигации ничего особо не поменялось.
Не совпали лишь некоторые нюансы:
- железяка была обнаружена как /dev/ttyACM0 вместо /dev/ttyUSB0
- при установке gpsd не нашел девайс самостоятельно, оказалось надо прописать в /etc/default/gpsd:
DEVICES="/dev/ACM0" - для python-3 gps пакет надо доставлять руками, он не идет вместе с gpsd
Сам девайс выглядит как обычная USB-флешка: 
А дальше я надолго застрял, так как мой GPS не видел координат.
Девайс работал, сообщал версию, производителя и прочую полезную информацию, но ничего больше.
Сначала я думал, что он все же бракованный. С прежних экспериментов у меня осталась SD-карта с Raspbian 2017 года, на котором GPS работал.
Загрузился с нее и — о чудо — GPS ожил и заморгал лампочкой, и посыпались в лог координаты.
Я ломал голову неделю, сравнил все конфиги, стал грешить на новый глючный Raspbian и уже готовился пересобирать в дебаге gpsd, как вдруг на Raspberry-форуме в одной из тем по проблемам GPS (а их там не мало) я набрел на совет вынести девайс на улицу в ясную погоду и дать ему найти спутники, что может занять до получаса.
Такая идея мне уже в голову приходила, но, видимо, погода была недостаточно ясная, а тут все совпало, и девайс ожил. Если копнуть чуть глубже, оказывается, что необходимая информация о спутниках кэшируется в прошивке девайса и не поддается контролю Raspbian.
How can my application get almanac/ephemeris/pseudorange data?
Sorry, there’s no easy way to do these things through GPSD yet. The reason is that there is no consistent way to make GPS receivers report this information.Many don’t ship it at all. Others (including some but not all devices shipping SiRF binary packets) ship it occasionally in SUBFRAME information, but you have to know exactly how to grovel through the SUBFRAME fields to get it and the documentation of those in IS-GPS-200E (the over-the-air protocol used by GPS satellites) is extremely obscure. Still others report varying subsets of almanac/ephemeris/pseudorange data in reasonably straightforward ways, but in vendor-proprietary sentences that are extremely specific to individual receiver types, poorly documented or undocumented, and often needing to be activated by control sequences that are equally specific and even worse documented.
Хитрый механизм заработал и настало время извлечь из этого пользу.
Скрипт для чтения данных с gpsd на питоне очень прост:
import gps
gs = gps.gps("localhost", "2947")
gs.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
for i in range(0,10):
report = gs.next()
print (report)
Google Maps API
Танком я управляю с телефона, так что пришлось изучать работу с Google Maps.
Гугл хорошо поработал над упрощением работы с картами, так что здесь проблем не возникло.
Android Studio умеет создать пустой проект с картой, откуда надо аккуратно перенести все важные детали в основной проект.
Гугл требует регистрации ключа для работы с Maps API, делается это бесплатно (пока).
Расширил REST-интерфейс танка, чтобы он отдавал свои координаты, передал координаты в карту, нарисовал танко-иконку и все выглядит вполне пристойно.
Дальше надо проложить путь.
Тычком на карте выбирается цель и скармливается в Directions API. Directions — это веб-сервис, который принимает координаты двух точек и в ответ вываливает кучу информации о проложенном маршруте, включая адреса, примечательные названия и пояснения. Но мне нужна была только последовательность шагов. Выбираем координаты и отрисовываем их на карте.
Теперь для визуального контроля и управления есть все необходимое.
GPS Навигация
Дальше с телефона на танк передается первая точка маршрута.
У танка есть небольшая проблема — в начальный момент времени, он не знает свое направление.
Проблема легко решается компасом, но с компасом то любой справится…
Удалось выкрутиться тем, что первые несколько секунд танк проезжает просто вперед, получая координаты начала и конца пути и вычисляя по ним свое направление. Стоит заметить, что эти же данные можно получить прямо с GPS, там есть поле track, которое и показывает отклонение от угла на северный полюс.
Но в любом случае, чтобы это после заполнялось, нужно движение.
Расчет направления:
def azimuth(pos1, pos2):
lat1 = toRadians(pos1["lat"])
lon1 = toRadians(pos1["lon"])
lat2 = toRadians(pos2["lat"])
lon2 = toRadians(pos2["lon"])
dlat = lat2 - lat1
dlon = lon2 - lon1
x = math.sin(dlon) * math.cos(lat2)
y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
return math.atan2(x, y)
Отмечу, GPS работает не всегда точно, и вполне может оказаться, что координаты начала и конца тестового пути будут с погрешностью, делающей измерения бессмыссленными (вплоть до того, что при движении вперед, координата конца пути оказывается сзади).
При этот GPS отдает направление через track довольно достоверно, поэтому это поле использовалось по умолчанию, а если его не было — приходилось ориентироваться по разнице координат.
Сориентировавшись на местности, танк поворачивает приблизительно куда надо (приблизительно — потому что без компаса или гироскопа точно измерить направление затруднительно) и движется несколько секунд. После чего опять получает координаты, сверяет направление, поворачивает, едет. И так до тех пор, пока цель не будет в радиусе погрешности.
В целом, самая большая проблема — погрешность GPS координат, из-за которой танк то и дело теряет направление и начинает метаться в разные стороны.
Все идет к тому, что без компаса не выжить.
Ссылки
- Вводная статья о настройке GPS модуля для Raspberry Pi
- Сервис для работы с GPS по Linux
- Полезный сайт с формулами расчета расстояния и направления по координатам двух точек
- Исходный код прошивки танка с поддержкой GPS
- Документация по Google Maps для Android
