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

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

Я как раз получил по почте несколько испытуемых, возможно скоро выложу сырые данные с них :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

Процитирую статью 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 не устраивает?

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