GPS с гироскопом и одометром.

Да, точно он.

Кстати для первичных экспериментов (с Калманом или любыми другими методами) думаю может подойти любой IBM/Lenovo Thinkpad (цена 4000-6000р. за приличный Core 2 Duo): у них у всех встроен двухосный акселерометр Analog Devices ADXL320. Это конечно не так круто как wheel tick и gyroscope, но для обкатки технологий тоже годится. И паять ничего не надо. Вычислительная мощность больше любого смартфона, геометрически выровнять в автомобиле проще, чем смартфон. Можно даже док-станцию прикрутить намертво - цена 400 рублей за простой повторитель портов или 1000-1500р. за докстанцию с настоящим (не USB) COM-портом, который висит на отдельном прерывании и годится для PPS.

Интерфейс для программистов: http://www.thinkwiki.org/wiki/Active_Protection_System
Видео: http://www.youtube.com/watch?v=lza19PUdoL8

У меня скопился целый зоопарк thinkpad-ов (T60, T61, X61, X61 Tablet), надо будет испытать зимой, когда времени побольше будет.

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

Рекомендую заказать MPU-6050 и USB TO I2C адаптер для подключения к ноутбуку. Будет 3-осевой акселерометр и 3-осевой гироскоп.
В сумме это обойдётся в $13.61, и паять кроме соединительных проводов ничего не надо.
Сам использую эту связку для экспериментов, и могу поделиться кодом на С++ для работы с ними (под винду).

Как раз центробежная сила (боковое ускорение) и является измеряемым параметром. Этот метод используется в топовых томтомах и некоторых моделях Pioner. Конечно точность у них не ахти, в основном для отслеживания развязок в тоннелях (map matching).

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

За наводки по электронике спасибо.

(added)
мда… почитал про точность и шум этих акселерометров. Для навигации не годятся, только для отслеживания “отвернул влево/вправо” без конкретных чисел. Но отследить вылеты GPS и забраковать измерения по ним можно. Собственно интерес возник исключительно после чтения литературы по Калману, хочется хоть что-то в него замешать минимальными усилиями :slight_smile:

Все на самом деле впаяно, и модуль нормальный LEA-6R, нет только второго кабеля для 5 пинового JST разъема:


$GPTXT,01,01,02,HW  UBX-G60xx  00040007 777FFFFFf*45
$GPTXT,01,01,02,EXT CORE 7.03 (47340) May 10 2011 14:36:32*47
$GPTXT,01,01,02,ROM BASE 7.03 (45969) Mar 17 2011 16:18:34*57
$GPTXT,01,01,02,MOD LEA-6R-0*37
$GPTXT,01,01,02,DR 6R C0 2.00*70

Похоже все несколько сложнее:
В базовой прошивке RXM-RAW есть, а вот в EXT все это дело
зачистили, остался только TRK-SFRB. Дело в принципе поправимое,
но не за 5 минут.

usm78-gis
Не смог найти поиском - каким адаптером вы пользуетесь для сниффинга CAN-шины ? Или у вас всё реализовано через arduino shields ?

У меня есть много всяких разных, но реально и всерьез пока пользуюсь (по разным причинам) только дорогим AGV4000B (expert) http://www.agv4000.de
на чипе LPC1766FBD100 http://www.nxp.com/products/microcontrollers/cortex_m3/LPC1766FBD100.html
разные причины = на A4 B7 шины CAN-Diagnostic и CAN-Powertrain это одна шина, поэтому доступ к датчикам колес и положения руля
можно получить прямо на OBD-2 разъеме. Более дешевые адаптеры предназначенные только для OBD-2 диагностики
на STM32F1* физически не могут запихать весь 500kbit CAN траффик
в 230kbit uart, а фильтрование CAN пакетов на дешевом железе непростое дело.
Arduino CAN shield в общем отличная вещь, но выход пакетов у него на SPI порт, так что тут еще кто-то вроде raspberry pi
нужен для обработки пакетов.

А смотрели ссылки про массивы из нескольких датчиков? Что думаете? По идее, там есть возможность повысить точность и снизить шум на порядок.

Я как раз получил по почте несколько испытуемых, возможно скоро выложу сырые данные с них :slight_smile:

Смотрел, интересное решение, но я не краевед в этой области :slight_smile:

Всем привет. Интересно, конечно, понаблюдать за темой, но многолетняя езда на LEA6-R как раз показывает, что чуваки из ublox прошли по всем-всем-всем граблям, которые обсуждались последние 3-4-5 страниц. Мое мнение касательно миксования gps и датчикоданных логично вытекает из этих обсуждений. Компас - ниочем. Никакой достоверной информации с него нет, не было и не будет. Акселерометры КРАСИВО РИСУЮТ МЕЛКИЕ ПОДРОБНОСТИ И МИНИМАЛЬНЫЕ РАДИУСЫ. Я вобщем только ради этого и занялся этой темой. GPS-данные в ВЧ диапазоне - просто шум, который ничего не говорит о движении автомобиля - координаты дрейфуют, шум по азимуту просто дурной по сравнению с датчиками. И дело не в точности GPS, и не в сырых данных, и даже не в герцах юблокса (привет SviMik). Дело в принципе. Спутники летящие с бешенной скоростью относительно земли и автомобиля НИЧЕГО конкретного об этом не сообщат, особено в ситуации ежесекндной смены списка видимых спутников в городе. Геодезисты еще могут както неторопливо пройтись от одной точки к другой и выколотить боле-мене точные относительно и абсолютно данные. В автомобиле, который двигается по городу повышать качество gps-приема - абсолютно тупиковый путь развития.

С другой стороны, если за основу взять треки с датчиков угловых акселерометров, то они сами-посебе дают отличные картинки. Очень красивые, четкие радиусы. С этих данных можно брать производные, и первую и вторую, и любую (gpsу превед). Фактически остается одна проблема - справится с тем что трек датчиков медленно-медленно уезжает.

Чем молодцы ublox? - они асилили фильтр калмана. Они не просто исправляют трек - они еще запоминают на сколько он был исправлен, запоминают поправочные коэффициенты, держат термостатированные таблицы [gyro bias] и [gyro scale factor]. Вот до этого местное обсуждение еще не дошло. Дальше, юблохи непрерывно следят за разницей позиций датчиков и gps и когда она достигает порога начинают сравнивать трек за последние несколько секунд. Если gps рисует правдоподобную картинку ОТНОСИТЕЛЬНЫХ перемещений похожих на датчики, то он передвигает АБСОЛЮТНЫЕ позицию и азимут в точку gps. Причем не скачком, а плавной загогулиной, чтобы юзеров не кошмарить, и субъективно-красиво выкручиваться из достадного положения.

Чем можно их объегорить - суперНЧ фильтром. Есть смысл из gps брать только самые крупные данные - отрезки прямых, с отсечением первых и последних 10%, где gps носит, можно взять кривые гигантских радиусов, в сотни метров и другие вещи, которые однозначно узнаваемы в треке от датчиков. Вот кому хватит на это терпения и сил - будет респект и уважуха.

Ура, товарищи. Извиняюсь за могабукв.
зы. группа НИОКР из огромного концерна TYCO тоже прошла по всем этим граблям. У них получилось лучше чем у юблохов, но они спеклись давно, сняли с производства чипы ДР. Еще навис и еще несколько групп делают попытки но ИМХО результаты скромны.

Данные с датчиков, которые на фотке #282 поста: http://svimik.com/imu_array_1.rar (37 мб)
+приклеил туда магнитометр.

В архиве два файла - запись с датчиков, и запись с GPS. В данный момент они не синхронизированы (единственное, начало и конец записи делались одновременно, так что на глаз прикинуть можно).

Главное нововведение (помимо того, что датчиков теперь 3) - общением с датчиками занимается контроллер, а не комп. Так что стало возможным делать очень точные временнЫе метки.

Новый формат лога:

492643 - текущее точное время в микросекундах (секунды\минуты не передаю, но их легко дорисовать самому, считая переход через ноль).

G0, G1, G2 - MPU6050. Благодаря наличию трёх независимых I2C интерфейсов, данные действительно снимаются синхронно, поэтому метка времени всегда одна. Далее идут: ACCEL_X, ACCEL_Y, ACCEL_Z, GYRO_X, GYRO_Y, GYRO_Z, и температура в градусах цельсия с 1 знаком после запятой.

MG - компас HMC5983 (x,y,z). Т.к. у компаса частота ниже, то данные идут реже, чем от IMU сенсора.

W0, W1 - датчики колёс. В примере выше (1,65535,85) 1 это число импульсов, 65535 - время* с момента предыдущего импульса до момента прихода этого импульса (не до timestamp лога! см. далее), 85 - время* с момента регистрации этого импульса (значение ненулевое, т.к. мгновенно залогировано быть не может, ибо процессор мог быть занят чем-то другим).

  • время здесь - единица считается за 15.25мкс.

Что нужно учитывать (далее просто будет К.О.):

  • Датчики не могут быть выставлены идеально относительно друг друга. Я постарался (на фотке видно), но погрешность по всем осям всё равно будет.
  • Вся эта конструкция целиком также имеет некоторые отклонения от осей автомобиля, ибо приделано на глаз.
  • Часы у МК свои, так что все временные метки хотя и имеют высокое разрешение, они не обязаны совпадать с атомными часами на GPS спутниках. Более того, они вообще никак с GPS не синхронизированы.
  • Данные передавались по USB-UART адаптеру, для него характерен некоторый мусор в самом начале файла (upd: в этом примере не попалось) :slight_smile: Желательно ввести хоть какую-нибудь проверку каждой строки на соответствие формату.

У них что и SCL линии синхронизированы ?
Я задумался над тем, как из этих данных вычислить 3 “калмановских” параметра,
используемых в модели ublox: $f_{T}, f_{\omega} и b_{\omega}$.
Для этого нужны радиус(ы) колес(а), независимые измерения скорости (gps?) и
скорости изменения угла поворота.
Сам я продал старую и купил новую машину, в ней нет такого сигнала
из каменного века как wheel tick :roll_eyes: ,
только 2 CAN шины на разделительном разъеме (CAN-Antrieb и CAN-Komfort),
и нет больше возможности считывать пакеты CAN-A с шины CAN-D (OBD-2),
а придется напрямую цепляться к пинам разъема CAN-A.
Так что LEA-6R теперь отдыхает.
Родной машинный навигатор MMI 3G (он же RNS850, и прочая) имеет собственный DR (Harman/Becker) и GPS приемник с
чипом antaris4, но там надо сильно хакать ОС QNX, впрочем я несколько продвинулся
и в этом направлении :sunglasses: (частично описано в RTKlib теме).

Edit Умельцы конечно продают и для этой машины “генератор wheel tick” из сигнала с CAN,
но он стоит больше приемника LEA-6R :stuck_out_tongue:

Таки вы правы. Разница во времени может составлять до одного SCL такта. Но думаю, на этом масштабе, читателя будет волновать уже другие факторы :slight_smile:

У меня ещё есть что улучшать в плане снятия данных, чтобы получать больше-быстрее-точнее, но я не буду продолжать развивать аппаратную часть, если это не будет кому-то интересно. Т.к. сам я уже сдался понять Калмана и Кватернионы, нужна помощь по части всего этого матана. От себя обещаю опубликовать схемы и прошивки под CC-BY-SA, если этот проект сдвинется с мёртвой точки. Но без решения математической части, сейчас это бесполезная груда железа, генерирующая бессмысленный поток цифр :slight_smile:

Я его даже не искал, мне было проще нащупать непосредственно датчик на колёсах, да и доверия к нему больше, ибо 100% не обработан никакими контроллерами, и не имеет никаких задержек.

Надо брать быка за рога и сразу делать калмана, так как описано u-blox’ом.
Но до этого надо предварительно разобраться с калибровкой датчиков и моделью часов.
Я так понимаю, что все I2C данные считываются STM32F4 по прерыванию таймера
и соответственно имеют метки времени часов STM32F4. Можно ли присоединть GPS к UART порту ?
NMEA будет тогда считываться по прерыванию UART и сравнивая метки времени GPS и STM32F4
можно будет составить модель дрифта часов STM32F4. Внешнее PPS прерывание пока трогать не будем :slight_smile:
Скорость для модели радиуса колеса берется тогда прямо из данных GPS, вот только с углом поворота
по данным GPS у меня геометрической фантазии не хватает.

Каков (примерный) радиус колеса и сколько “тиков” датчик дает на 1 оборот колеса ?

В данный момент это происходит в основном цикле. Хотя могу сделать и по прерыванию.

У меня в наличии только USB приёмник. Хотя учитывая неспешность интерфейсов у GPS, может с этим справится и комп?
При 4800 бод, пока GPS передаст, что у него есть, пройдёт вечность :slight_smile:
Здесь PPS прерывание как раз имело бы больше смысла (и реализуется элементарно), если бы такой выход у моего приёмника был.

Это конечно круто! Но может попробуем пока без этого? :slight_smile:

Экспериментальным путём мы узнали, что первое (левое) колесо выдаёт 50.4 импульса на метр, а второе (правое) - 50.17. Колея автомашины - 1.41 метра.
В терминах радиуса и оборотов сейчас не скажу. Если надо - могу померить.

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

Число тиков на оборот величина абсолютная, так как задается устройством железа:
числом зубов и т.п.
“Измеряемым” параметром в модели ublox
является средняя скорость, которую они и советуют брать с задних колес.
В модельном плане важнее как определить скорость изменения угла гироскопа
по данным GPS (ну или по данным для угла поворота руля).
Это чисто геометрические задачи.

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

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

  1. Откалибровать смещение (offset) всех осей всех датчиков. При необходимости также отмасштабировать (scale), и построить температурную таблицу этих параметров.
  2. Используя немного геометрии, повернуть оси датчиков (благо они все 3-осевые) таким образом, чтобы получить виртуальные датчики, расположенные в одном направлении.
  3. Объединить массивы гироскопов и акселерометров в один виртуальный гироскоп и акселерометр (алгоритмы уже более-менее известны, статьи есть, осталось реализовать).
  4. Для датчиков колёс также произвести калибровку числа импульсов на пройденный путь, и расстояние между колёсами (колея).
  5. Закинуть все датчики в фильтр Калмана, далее идёт какое-то волшебство, работа с кватернионами, и мы получаем текущее расположения автомобиля в пространстве.
    ---------(немного помечтаем, что может быть дальше, если меня досюда подбросят)---------
  6. Портирование кода на конечную платформу, чтобы все вычисления выполнялись на МК и\или embedded платформе (пока положил глаз на связку STM32F4 и Raspberry Pi для высокоуровневых задач, благо обе железки достаточно дешёвые в своих областях применения).
  7. Интерфейс для подключения ПК с симулятором NMEA (для совместимости с обычным софтом) и расширенным набором параметров (для своего софта).
  8. Прочие интерфейсы (например, дисплей с компасом и искусственным горизонтом для украшения автомобиля), слежение через 3G, запись в чёрный ящик, и т.п.
  9. Корпус (есть возможность печатать на 3D принтере), создание законченного девайса.
  10. Применение для различных задач навигации и сбора информации. Маппинг поздемных парковок, тоннелей, видеомаппинг, навигация по полосам, и т.п.

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

SviMik
Прикол Калмана в том что для него все калибровочные коэффициенты (температурные, развороты осей, изменяющийся радиус колес и пр.) впоследствии можно вынести в алгоритм как “неизвестные”, только с очень длинным low-pass фильтром. Но даже для Калмана п.п.1-4 из вашего описания, безусловно, нужны и важны для первой итерации, иначе можно годами ждать когда оно всё устаканится (а может и пойти вразнос). Но единожды задав хорошие начальные значения, в дальнейшем они могут жить своей жизнью.

Наши предыдущие споры заставили меня сесть и перечитать теорию. Раньше я постоянно сравнивал фильтр Калмана с уравниванием МНК (видимо у нас в морских нав.системах используется какая-то комбинация - часть работает на Калмане, часть на LSA). Оказалось всё намного интереснее и разнообразнее и общего у них только то, что оба минимизируют невязки. Хотя очень и очень по-разному.

В процессе поисков наткнулся на интересный проект OpenPilot для коптеров. Там довольно подробно описан алгоритм и модель при работе с гироскопами (у них фильтр работает на частоте 1000Hz), а также есть прошивка контроллера в исходниках и toolchain. Может найдёте что-то интересное для п.5 из вашего роадмапа.
http://wiki.openpilot.org/display/Doc/INSGPS+Algorithm