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

Процитирую статью u-blox
“Low-Cost Sensor Fusion Dead Reckoning using a
Single-Frequency GNSS Receiver Combined with
Gyroscope and Wheel Tick Measurements”:

                                 
The speed v and heading rate Hd' are derived from wheel
tick and gyroscope measurements as follows:
               
   v =  Traw*2\pi/(Tn*dt)*r*f_T     (1)
                             
    Hd' = fw (w - bw )                 (2)
with
         Traw     ...       wheel ticks measured
         r        ...       wheel radius
         Tn       ...       ticks per wheel turn
         dt       ...       time period
         w        ...       gyroscope reading

The wheel tick factor f_T accounts for changes of the wheel
radius and road surface unevenness which is the largest
error source with respect to the velocity derived from
wheel ticks. Gyroscope scaling factor fw and bias bw
consider the characteristics of the gyroscope sensor.

Например, мой фактический (статический) радиус колеса ~320мм (225/50R17), т.е. 2\pi*r ~= 2010mm
(порядка 2 метров на оборот). Число тиков Traw за период времени dt меряется, число тиков на оборот Tn известно.
Скорость v при езде по прямой (Hd’=0) можно взять из данных GPS. Таким образом определяется фактор f_T
и его статистические параметры (среднее значение, стандартное отклонение).
С этого и стоит нАчать.

Никакого волшебства, все делается тупо как описано в статье.
Фактически из (двумерного) вектора скорости v(t) + направления Hd(t) в данный момент времени t определяется путем 1 интегрирования
вектор положения (x(t),y(t)). Абсолютно элементарная задача в смысле физической модели, Калман здесь нужен только для
борьбы с “шумом”.
Возможное расширение это учет данных о радиусе кривизны траектории (GPS+угол поворота руля).
Не хватает только элементов ковариационных матриц,
я думаю для начала надо будет ограничиться диагональными элементами, т.е. стандартным отклонением для нормально
распределенного шума. Которые надо брать или из документации, и/или определять экспериментально.

Хотелось бы все таки узнать радиус колеса, и
иметь еще 2 раздельных файла: стационарный (машина стоит) и
движение строго по прямой (с синхронизированными
по времени данными GPS)

Какая длительность нужна?
Можно взять кусок в начале, вырезать до первого сигнала датчика колеса.
Если надо больше - запишу отдельно.

Остальное - будет позже.

Погляжу. Вообще надо бы время в логе привести от таймера к нормальному
ISO8601 виду (пусть пока и с дрифтом).
В NMEA логе достаточно $GPGGA.

SviMik, нужны калибрационные данные гироскопа и акселерометра для используемого
устройства, и соответственно о системе единиц для измеренных параметров.
u-blox опубликовал на днях новый каталог v15 (u-blox_product_catalog_15.pdf), в нем есть данные о том, что можно ожидать от ublox M8 3D DR (UBX-M8030-Kx-DR)


Sensor option                Typical position error3
 Rear wheels (2D):                         12%
 Front wheels (2D):                        13%
 Four wheels (2D):                         10%
 Gyro + speedpulse (2D):                    5%
 Gyro + speedpulse + accelerometer (3D): <5%

и также требования к сенсорам


Sensor requirements
 Wheel tick:               Resolution better than 2 cm/tick4.
 Wheel info:               Free from deadband behavior and 
                           linear with wheel rotation.
 Gyro (optional): Accuracy:                       < 0.1°/s
                           Dynamic range: ±100°/s
                           Linearity:             ±0.5°/s (full scale)
 Accelerometer: Linear acceleration measurement 
      (optional)           range: ± 4g
                           Linear acceleration sensitivity: 8mg
4
   Resolution better than 50 cm/tick in combination with a yaw gyroscope.

Слово “CAN” нигде не упоминается, но фактически присутствует.
Предполагается, что некий “application processor” будет поставлять CAN данные для u-blox:


The digital data provided by the sensors is con-
verted to proprietary UBX messages by the application
processor.

ИМХО оптимистичное предположение, так как из-за явной нехватки вычислительных ресурсов
“20 Hz” достигаются откровенно жульническими методами:


Max. nav. update rate  20 Hz (1)
...
1) For update rates > 5 Hz, extrapolation techniques are applied

Harman/Becker, как я писал, тоже сами решают задачу DR, не полагаясь на ublox.

Добрый день. Проблемой комплексирование gps и других одмоетрическо-инерциальных приборов занимаюсь уже год, есть два прототипа собранных на коленке. Ядром платы является stm32f4dicovery, gps Neo-6m, и пока 3 инерциальных датчика (гир, магнитометр, аксель). Сейчас планирую делать нормальную плату логгер/анализатор/высчитыватель по подобию http://www.kickstarter.com/projects/richardhaberkern/gps-cookie-leaving-crumbs-wherever-it-goes?ref=live плюс инерциальные датчики. Хотел спросить, какой из gps модулей посоветуете, но так чтобы можно было достать и не стоил как тысяча убитых енотов? кому интересно могу скинуть апноуты для калибровки магнитометра и акселя. С гиром пока не совсем вся ясно.

А чем neo-6m не устраивает?

Самый важный датчик…
Какая модель ?

L34200d, mpu 9150, itg3200. Neo -6m вполне устраивает. Думал может есть че получше за эти деньги

О!
Спасибо за ссылку!
Готовый “гпс с гироскопом и одометром” за 95$ - именно то, что нужно.

Как раз её и использую :slight_smile:

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

Если кому интересно - могу выложить код для stm32f4dicovery, читающий паралельно 3 датчика MPU-6050, и в перерывах между делом - компас HMC5983. При этом читает два датчика на колёсах (ABS), и всё это шлёт в UART.

Одна из идей была накладывать карту и подсказки на изображение с камеры в реальном времени с минимальной задержкой. Чтобы виртуальный слой и видео не разъезжались, нужно следить за движениями автомобиля с частотой не в 5, а 25-50гц, что никакой “гпс с одометром”, сделанный на u-blox, не может. Просто потому, что в u-blox есть искуственное ограничение в 1-5гц, при том, что скорострельность MEMS датчиков обычно составляет сотни герц, и ничто не мешает обсчитывать координаты на той же скорости, получая прекрасный отклик на изменение расположения автомобиля в пространстве.

лучше neo-6m с внешней антенной только nv08c-csm с внешней антенной. itg3200 один из самых дорогих
гироскопов, можно конечно спорить о том, нужны ли его возможности для автонавигации.
у меня тоже такой есть, так что можно предметно обсуждать.
подключить я бы его хотел к raspberry pi или pcduino.

Желающие есть, но у желающих есть работа, которая ко этому всему делу никакого отношения не имеет,
и отвлекает :slight_smile:

оно касается только GPS, и там оно скорее естественное, так как процессор слишком медленный.

посмотрите повнимательнее, там нет одометрии. Только GPS. Или я ошибаюсь?

Тема не заглохла, я готовлю алгоритм на нечёткой логике и нейросетях. В декабре планирую провести тест на irobot. А потом на живой машине.

Было бы хорошо увидеть исходники, или алгоритм работы ввиде блок схемы.

Спасибо, посмотрю что за штука.

чем дальше в лес тем больше щепок=))

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

Может ещё что-то посоветуете засунуть в плату?

ITG3205 стоит $9.80 на ебее (интересно, это тоже самое, или нет?)

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

Я в данный момент остановился на MPU-6050, как на наиболее навороченном (гироскоп и акселерометр в одном чипе). Кстати, тот же производитель, что и ITG3200.

Можете пояснить? Насколько я понимаю, 1-5гц - это та скорость, с которой они способны выдавать данные, независимо от того, задействоана функция dead reckoning или нет.

Использовать малинку - примерно также, как использовать комп с адаптером USB-I2C. Для проверки концепции можно, но финальное устройство я бы так не делал.

Работу с датчиками удобно осуществлять с stm32f4dicovery - недорогая плата с хорошей производительностью, и аж тремя I2C интерфейсами.
При необходимости, малинку можно использовать для высокоуровневых задач (wifi\3g\дисплей\камера).

Моя мысль была - сделать инерциалку в виде отдельного законченного устройства с USB шнурком. И использовать для этого stm32f4dicovery.
Далее, пользователь сам выбирает, как использовать - с ноутбуком, NUC, смартфоном, или малинкой, в зависимости от его задач.

mpu 9150 тоже что и 6050, только с магнитометром. А можно код инициализации mpu под stm32f407

Ну вот я как раз сейчас такую штуку делаю. И еще для тех у кого нет возможности собирать платы, чёто-то куда-то подключать. Можно использовать телефон. Android или Iphone. проверял на sgs 3 и iphone 4. там и там 3 датчика, даже вроде 4 и gps. http://www.youtube.com/watch?v=-k–3GxrQXU
http://www.mathworks.com/matlabcentral/fileexchange/40858-iphone-and-ipad-sensor-support-from-matlab

Чё наколупал по ссылке на последний ролик! прожка под андроид, рисует датчики на карту (или шлёт в вафлю), Sensorstream IMU+GPS, буду пробовать…

В теории можно тел прикрепить на голень ближе к стопе и обходить помещения/магазы и мапить indoor. Главное, не на входе в магаз надевать конструкцию, а то секурити “возьмёт” тут же))
как-то всё это еще обрабатывать надо… :confused:

UPD: блин, облом, гироскопа-то и нет, а без него же никак? на акселях, типа…

UPD: нарезал csv, кто-нибудь поможет разобраться из математиков? реально из него что-то вытащить?
с километр пешего хода, тел в нагрудном кармане, в зипе 3 МБ. Режим съёма “fastest”, может, как-то шаги вместо одометра вычислить и их применить, юстировав по гпс? шёл равномерно, не вертелся))
Плюс gps для “поддержки” там включен, вот: csv лог-файл.
По идее, последовательность датчиков, как на скрине выше, но там чё-то такая каша, как я глянул. Может, какие-то датчики вторичны, и следуют из неких основных?

Может, какие маткады на это можно натравить, совершенно не разбираюсь…(( Биения шага как-то вычислить, увязать с гпс…?

Или забить

Если что-то выгорит, можно мапить в зданиях)

Можно. Я так понял, нужно только чтение сенсоров? Если DMP не нужен, то тогда всё просто:
http://svimik.com/invensense.h
http://svimik.com/mpu6050.c
mpu6050_init - если хотите читать напрямую из регистров
mpu6050_init_fifo - если надо инициализировать вместе с FIFO
Пример вызова: mpu6050_init(I2C1, GYRO0)

Для чтения напрямую, все регистры найдёте в invensense.h
Для чтения из FIFO - сначала проверять mpu6050_fifo_overflow, если переполнился - сбрасывать через mpu6050_fifo_reset. Далее, получить количество байт через getFIFOCount, и можно начинать читать байты из регистра MPU6050_FIFO_R_W.

Весь код поддерживает одновременную работу с тремя i2c линиями (I2C1, I2C2, I2C3 - передаётся при вызове в каждую функцию). Таким образом, я подключал 3 датчика сразу.

Только, функции для работы с i2c заменить на свои:

#define mpu6050_get(reg) read(I2Cx, dev_addr, reg)
#define mpu6050_set(reg, value) write(I2Cx, dev_addr, reg, value)

Код проекта полностью: http://svimik.com/mems_array_src_v1.rar
(если какой файл забыл - скажите. Всю папку проекта не кидаю, т.к. много мусора - выбрал нужные файлы вручную).

Возможности:

  • Работа с сенсорами колёс (схема подключения для skoda fabia была где-то в этой теме N страниц назад)
  • Паралельное чтение трёх MPU-6050 по трём i2c линиям.
  • Автоматическая реинициализация всего и вся в случае сбоя
  • Запись в UART с использованием DMA
  • компас HMC5983 там тоже есть

Disclaimer:
Это прототип. Я не пытался следовать каким-либо правилам\стандартам написания кода, не пытался оптимизировать. В коде много отладочных затычек, повсюду расбросаны printf, царит хаос, и всё такое :slight_smile:
Сам МК ничего не высчитывает (ну разве что, некоторые оффсеты для датчиков вручную забиты), а на данном этапе шлёт на комп (по UART) в целях логирования и дальнейшей отладки.

Судя по цене это китайская некондиция. Где гарантия соответствия параметров спецификации, и
работы пару лет ?

16битный + большой динамический диапазон + низкие параметры собственного шума.

ublox DR осуществляет “фузионирование” сырых данных GPS с данными о скорости + гироскопа.
atmel процессор просто по техническим причинам в большинстве случаев не справляется с обработкой сырых данных GPS со скоростью > 2Hz,
если программа и данные находятся во внешнем флэше. ublox7 имеет процессор cortex m3 (очень похож на STM32F1) и поэтому
справляется c 10 Hz (но добрые души из ublox выкинули функции выдачи сырых данных из прошивки, хотя есть некая надежда на NEO-7P).
Таким образом, двумя входными DR параметрами является абсолютная скорость (m/s) и скорость изменения угла от гироскопа (°/s).
В алгоритме DWT абсолютная скорость берется с CAN шины, а скорость изменения угла высчитывается из угла+скорости изменения угла поворота руля
(тоже с CAN шины). Тут тактовая частота обновления данных задается CAN шиной (т.е. 10 и 20 ms).
В алгоритме GWT скорость оцифровывается измерением длины импульсов скорости, а скорость изменения угла от гироскопа с ADC на SPI порту
(скорость что-то вроде 500 kHz, точно не помню, но ее легко померять осциллографом на LEA-6R).
Эти данные и интегрируются в периодах между GPS измерениями (тактовая частота 1 Hz) и скармливаются EKF, описанному в статейках.
Я все формулы перегнал в latex, и там есть некие темные места (например откуда берется pitch Pt, или “фактическое” отсутствие вектора {u} перенесенного в вектор {x}), но основная идея вполне понятна
и может быть реализована. Фактическое “ноу-хау” содержится в элементах ковариационных матриц, но их и надо будет определить
эмпирически на основании реальных измерений.

http://www.micronika.ru/order.phtml?keyword=NV08C-CSM


00259898	 	 	 	NV08C-CSM модуль Глонас	1656.00 на заказ	

но имхо лучше сразу ориентироваться на Globalsat TR-600 GLONASS http://www.globalsat.ru/catalog/tr-600-glonass
особенно если для вас не проблема собрать прошивку для STM32F103. Технические детали описаны тут в соседней ветке.

Не знаю как там с RTlinux для arm, но отлаживать на linux проще чем на RTOS и вообще для контроллеров с гигагерцовыми
процессорами эта проблема преувеличена. Вы же не будете параллельно крутить 3D картинки без железного opengl или
писать на software raid6 состоящий из набора флешек :slight_smile:

Модем, см. выше Globalsat TR-600 GLONASS