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

SviMik · 2013-08-16 23:32:40

Трэк 17 только по одометру
http://ge.tt/8zXzcRS/v/41?c

Пробовал решить эту задачу, вышло что-то вот такое

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

Колёсная база и диаметр колёс подбирались вручную, или вывелись автоматически?

Last edited by SviMik (2013-08-16 23:40:51)

OverQuantum · 2013-08-17 09:29:15

SviMik wrote:

Колёсная база и диаметр колёс подбирались вручную, или вывелись автоматически?

Подбирались. Диаметры явно разные (кол-во импульсов на метр с датчика), иначе график разности хода колёс имеет явный уклон, а трек становится вот таким, состоящим из дуг.

SviMik · 2013-08-17 10:02:25

Вместо HMC5883L заказал более продвинутый компас HMC5983. С температурной коррекцией и автоматической компенсации оффсета. Посмотрим, будет ли толк.

SviMik · 2013-08-17 11:15:05

Наткнулся на интересную идею:

1. Интегрированный массив МЭМС гироскопов с повышенной стабильностью показаний
2. Signal Processing of MEMS Gyroscope Arrays to Improve Accuracy Using a 1st Order Markov for Rate Signal Modeling (pdf версия, вроде более полная).

Для тех, кому лень читать, сразу результаты для 6 гироскопов:

Результат позитивный, т.к. даже Simple average дал улучшение более чем в 2 раза (а это именно тот алгоритм, который я в состоянии сделать самостоятельно ).

Достоинства подхода:
- Можно недорого улучшить точность всех сенсоров (гироскопы, акселерометры). MPU-6050 стоит менее 4$, а в нём сразу и то и другое в одном чипе.
- Способ выглядит простым (для Simple average), с возможностью дальнейших улучшений алгоритма (если повторить статью в полном объёме).

Недостатки:
- Придётся отказаться от MotionFusion алгоритма, зашитого в MPU-6050, и считать всё вручную.

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

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

Теперь вопрос: есть желающие?
Попробовать матан с DCM можно уже сейчас (в логах есть сырые данные акселерометра, гироскопа, компаса, и текущая скорость для компенсации центробежной силы). Позже останется только заменить гироскоп на виртуальный, более точный. Кстати, с акселерометром и компасом можно проделать тоже самое.

OverQuantum · 2013-08-17 11:59:24

SviMik wrote:

- Придётся отказаться от MotionFusion алгоритма, зашитого в MPU-6050, и считать всё вручную.

Я уже рублю ручной рассчёт постепенно.
Вот скажи, сколько были AFS_SEL и FS_SEL?
А то акселерометр в покое (начало и конец) показывает силу по модулю около 6916 отсчётов, тогда как g должно быть 8192 или 16384 или 4096 или 2048. Если AFS_SEL=1, то акселерометр показывает что g было ~ 8.27 м/с^2. Это меньше, чем на Венере и примерно как на высоте 600 км от Земли

Last edited by OverQuantum (2013-08-17 12:05:29)

SviMik · 2013-08-17 12:16:59

Тяжело сказать. При использовании MotionFusion я брал значения, которые выдавал он сам, обещая их сырыми. Что там на деле - я не знаю, код закрытый...
Тут либо попробовать принять 1g за 6916 и посмотреть что выйдет, либо план Б.

План Б: Если надо, могу записать трек без MotionFusion. Тогда, могу выдать действительно сырые данные с частотой 1кгц. Но в логе уже не будет кватернионов, курса, крена, тангажа и т.п. - ручной рассчёт станет обязательным условием

SviMik · 2013-08-17 20:21:06

План Б, для ручного рассчёта:

http://svimik.com/imuraw_1.rar (571кб) - Уровень сложности: простой, аналогичен треку 17.
http://svimik.com/imuraw_2.rar (894кб) - Уровень сложности: сложный, езда в помещении как трек 18.

Частота: гироскоп\акселерометр: 200Гц, компас и колёса: 100Гц.
Порядок чтения: $IMU, $MAGR, $IMU, $WRAW.
$IMU может представлять несколько пакетов подряд из-за наличия буффера у MPU-6050, который я читаю целиком за раз.
Настройки: AFS_SEL=0 (2g), FS_SEL=0 (250°/s). Компас: -2048...2047 соответствует +-0.88 Ga.
В начале и конце немного постоял для статистики.
Начало и конец обоих треков - одно и то же парковочное место, т.е. должны совпасть.

OverQuantum · 2013-08-17 20:26:17

$IMU -704,120,14016,-33,19,-168
$MAGR -192,-108,-374

Кто все эти числа?

Если в $IMU сначала идёт 3 оси акселерометра, то по показаниям получается ~0.86 g.

Last edited by OverQuantum (2013-08-17 20:32:45)

SviMik · 2013-08-17 20:39:01

$IMU - данные из FIFO буффера MPU-6050. Первые 3 похожи на акселерометр, и следующие - на гироскоп
Предположительно ACCEL_X, ACCEL_Y, ACCEL_Z, GYRO_X, GYRO_Y, GYRO_Z.

$MAGR - магнетометр. X, Z, Y.

$WRAW - с ним читатель уже знаком

Если в $IMU сначала идёт 3 оси акселерометра, то по показаниям получается ~0.86 g.

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

Исходный кот:

short imu[]={0,0,0,0,0,0};
short magn[]={0,0,0};

while(app_running){
	/* MPU6050 */

	// check FIFO overflow
	if(mpu6050_fifo_overflow()){
		printf("FIFO overflow!\r\n");
		fprintf(log, "$ERR overflow\r\n");
		mpu6050_fifo_reset();
		continue;
	}

	// get FIFO count
	int fifo_count=getFIFOCount();
	if(fifo_count>1024){
		printf("Wrong FIFO count (%i)!\r\n", fifo_count);
		mpu6050_fifo_reset();
		continue;
	}
	int frames=fifo_count/12;
	if(frames==0){continue;}
	
	// get timestamp
	getdate(&date);
	if(date.wMilliseconds!=prevdate.wMilliseconds || date.wSecond!=prevdate.wSecond){
		fprintf(log, "[%02i:%02i:%02i.%03i]\r\n", date.wHour, date.wMinute, date.wSecond, date.wMilliseconds);
	}
	prevdate=date;

	// read FIFO
	unsigned char tmp[1024];
	read(GYRO_ADDR, MPU6050_FIFO_R_W, tmp, frames*12);

	// parse data
	for(int i=0;i<frames;i++){
		unsigned char * rbuf=tmp+(i*12);
		imu[0]=(rbuf[0]<<8)|(rbuf[1]&0xFF);
		imu[1]=(rbuf[2]<<8)|(rbuf[3]&0xFF);
		imu[2]=(rbuf[4]<<8)|(rbuf[5]&0xFF);
		imu[3]=(rbuf[6]<<8)|(rbuf[7]&0xFF);
		imu[4]=(rbuf[8]<<8)|(rbuf[9]&0xFF);
		imu[5]=(rbuf[10]<<8)|(rbuf[11]&0xFF);
		fprintf(log, "$IMU %i,%i,%i,%i,%i,%i\r\n", imu[0],imu[1],imu[2],imu[3],imu[4],imu[5]);
	}

	if(++tick2%2==0){
		/* wheel sensors */
		unsigned char ss_cnt[2];
		unsigned int ss_interval[2], ss_delay[2];
		if(!speedsensor_get(ss_cnt, ss_interval, ss_delay)){printf("Bad packet\r\n");}
		fprintf(log, "$WRAW %i,%i,%i,%i,%i,%i\r\n", ss_cnt[0], ss_cnt[1], ss_interval[0], ss_interval[1], ss_delay[0], ss_delay[1]);
	}else{
		/* compass */
		compass_read(&magn[0], &magn[1], &magn[2]);
		fprintf(log, "$MAGR %i,%i,%i\r\n", magn[0], magn[1], magn[2]);
	}
}

Last edited by SviMik (2013-08-17 20:47:59)

SviMik · 2013-08-17 22:54:45

Есть ещё вариант попробовать ITG-3205 + ADXL345 вместо MPU-6050.
Минусы:
- В MPU-6050 гироскоп и акселерометр синхронизированы (оцифровываются одновременно), пара раздельных сенсоров будут работать асинхронно.
- В MPU-6050 встроенный таймер и свой буффер, дающий фиксированную частоту данных (независимо от лагов принимающей стороны). У других сенсоров будет просто чтение регистров в цикле, и QueryPerformanceCounter на каждое сообщение.

Плюсы:
- Можем получить адекватное 1g
- Шанс сравнить два сенсора с разными подходами.

OverQuantum · 2013-08-17 23:28:25

Ну я ещё не добрался до ручного интегрирования гироскопа.
Но экспериментируй, если хочешь

SviMik · 2013-08-18 12:11:05

Вариант с ITG-3205 + ADXL345: http://svimik.com/accelraw2_1.rar (6.71мб)
Формат:
$A %f,%i,%i,%i - time interval (us), accel x, y, z
$G %f,%i,%i,%i - time interval (us), gyro x, y, z
$W %f,%i,%i,%i,%i,%i,%i - time interval (us), wheel sensor data
$M %f,%i,%i,%i - time interval (us), magnetometer x,z,y

Порядок следования: фиксированный, AG, AG, W, AG, AG, M
Частота: плавающая, порядка ~500Гц.
Временные метки: на каждой строке, прошедшее время в мкс относительно предыдущей строки.

OverQuantum · 2013-08-18 14:17:56

SviMik wrote:

$IMU - данные из FIFO буффера MPU-6050. Первые 3 похожи на акселерометр, и следующие - на гироскоп
Предположительно ACCEL_X, ACCEL_Y, ACCEL_Z, GYRO_X, GYRO_Y, GYRO_Z.

В общем, что-то странное с этими данными.
GYRO_Z явно имеет постоянную составляющую в покое - ~ -173, GYRO_X - ~39.
Интегрировать показания гироскопа получается, но только если умножать на коэффициент. Для imudata17.rar это 1.44 (если считать что показания в градусах в секунду), для imuraw_1.rar - 1.05 (если считать, что показания - 131 LSB / градус в секунду). Причём результат интегрирования всех трёх осей хуже, чем только Z. Коэффициент нужен помимо преобразования градусов в радианы, времени в секунды и т.п.
ACCEL_X, ACCEL_Y тоже имеют постоянную составляющую, но это может быть обусловлено ещё и наклоном чипа относительно горизонтали. Тут я не понимаю как выделить разложение g на 3 оси и как - постоянные составлющие показаний чипа. В результате не могу даже уверенно определить, куда машина разгоняется - вперёд или назад.
По спецификации не могу понять, допустимы ли эти постоянные составляющие и коэффициенты или нет.

Буду пробовать новые данные.
В крайнем случае можно будет оставить только GPS и одометр.

Last edited by OverQuantum (2013-08-18 14:22:36)

SviMik · 2013-08-18 14:43:19

GYRO_Z явно имеет постоянную составляющую в покое - ~ -173, GYRO_X - ~39.

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

Для imudata17.rar это 1.44 (если считать что показания в градусах в секунду), для imuraw_1.rar - 1.05 (если считать, что показания - 131 LSB / градус в секунду).

В первом случае, возможно, дело в потере пакетов. Во втором случае я поработал над этим, потерь быть не должно. 1.05 уже похоже на погрешность самого сенсора.

ACCEL_X, ACCEL_Y тоже имеют постоянную составляющую, но это может быть обусловлено ещё и наклоном чипа относительно горизонтали.

Ясен пень, наш мир не идеален

По спецификации не могу понять, допустимы ли эти постоянные составляющие и коэффициенты или нет.

http://www.invensense.com/mems/gyro/doc … -6000A.pdf
Страницы 12 и 13.
Если правильно понимаю, то коэфициенты это Sensitivity Scale Factor Tolerance, домустимо +-3%, что впринципе близко к 1.05.
Есть ещё Initial ZRO Tolerance. Не знаю, что это, но возможно постоянная составляющая. Там аж ±20º/s.

OverQuantum · 2013-08-18 15:57:16

SviMik wrote:

$M %f,%i,%i,%i - time interval (us), magnetometer x,z,y

А какой магнетометр?

SviMik · 2013-08-18 15:58:17

Во всех логах HMC5883L.

OverQuantum · 2013-08-18 16:37:29

Не, извини, нифига не понимаю, что с этими данными делать.
Почему магнетометр по оси Z показыват самое большое значение и больше всего плавает?.. Почему модуль вектора магнитного поля плавает от 300 до 650?.. Плюс не ясно, какая калибровка, какой режим, какое расположение датчиков относительно друг друга, какое закрепление на машине (жёсткое или лежит на сидении) и т.п.

Буду пока скрещивать одометр с GPS

SviMik · 2013-08-18 16:44:23

Почему магнетометр по оси Z показыват самое большое значение и больше всего плавает?

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

Почему модуль вектора магнитного поля плавает от 300 до 650?

Не калиброван. У датчика на каждой оси есть смещение, с ним надо разбираться самостоятельно.

какое закрепление на машине

нифига не понимаю, что с этими данными делать.

Last edited by SviMik (2013-08-18 17:07:25)

kapilov · 2013-08-19 09:40:03

SviMik wrote:

Не калиброван.

А если откалибровать будет четко показывать?

Почему нельзя брать информацию только с компаса и датчиков на колесах?

SviMik wrote:

вертикален на полюсах

На магнитных полюсах.

SviMik · 2013-08-19 10:49:27

А если откалибровать будет четко показывать?

Да, при условии, что перед вычислением курса, вектор компаса надо будет вправлять на место в соответствии с креном\тангажом автомобиля, ибо компас работает только в строго горизонтальном положении (но к счастью, т.к. у него 3 оси, горизонтальное положение можно получить виртуально).

Но калибровка должна производиться автоматически и с некоторым интервалом, иначе может уплыть уже через час. Надеюсь, замена датчика на более продвинутый решит эту проблему.

Почему нельзя брать информацию только с компаса и датчиков на колесах?

1. См. выше. Нужна информация о наклоне автомобиля, иначе вычисление курса не будет работать корректно.
2. Если я положу рядом телефон\ноут\любой девайс с магнитом внутри, курс будет сбит сразу, и до тех пор, пока алгоритм калибровки не обнаружит сей досадный факт. Как быстро это произойдёт - зависит от алгоритма.
3. Почему в список не включен GPS? Колёса тоже не идеально показывают расстояние, погрешность может достигать 2%.
4. Как учитывать наклон дороги? Колёса справедливо выдадут, что проехали больше, чем это отобразится на плоскости карты.

На магнитных полюсах.

Капитан Очевидность

Last edited by SviMik (2013-08-19 10:54:24)

SviMik · 2013-08-19 11:04:09

Иллюстрация "не все дороги горизонтальны".

kapilov · 2013-08-19 12:54:55

SviMik wrote:

2%

У вас запросы больше, чем я думал.

SviMik · 2013-08-19 22:01:39

У вас запросы больше, чем я думал.

Всмысле? На 10км 2% будет уже 200 метров мимо. От переодической синхронизации с GPS никуда не денешься, независимо от запросов.
Или я вас неправильно понял?

PS. А что делать зимой, когда колёса проскальзывать могут?

Last edited by SviMik (2013-08-19 23:03:25)

kapilov · 2013-08-20 08:15:25

Конечно, от синхронизации никуда не денешься.

Я мыслю в том направлении, что хорошо бы было если во время стоянки на светофоре, при проезде арок, развязок, туннелей и тп. координаты не бросало в стороны. А арок и развязок в 10 км длинной нет (туннели есть, но не так много).

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

SviMik wrote:

зимой, когда колёса проскальзывать могут?

А есть возможность определить, какое именно колесо буксует? При разгоне брать наиболее медленное, при торможении - наиболее быстро вращающееся колесо.

pfg21 · 2013-08-20 08:24:37

интересное обсуждение, а если все четыре колеса сорвало в юз ??
вы так до доплерового датчика, направленного на дорогу, доиграетесь.

Last edited by pfg21 (2013-08-20 08:25:38)

Announcement

#226 2013-08-16 23:32:40

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

#227 2013-08-17 09:29:15

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

#228 2013-08-17 10:02:25

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

#229 2013-08-17 11:15:05

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

#230 2013-08-17 11:59:24

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

#231 2013-08-17 12:16:59

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

#232 2013-08-17 20:21:06

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

#233 2013-08-17 20:26:17

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

#234 2013-08-17 20:39:01

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

#235 2013-08-17 22:54:45

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

#236 2013-08-17 23:28:25

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

#237 2013-08-18 12:11:05

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

#238 2013-08-18 14:17:56

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

#239 2013-08-18 14:43:19

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

#240 2013-08-18 15:57:16

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

#241 2013-08-18 15:58:17

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

#242 2013-08-18 16:37:29

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

#243 2013-08-18 16:44:23

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

#244 2013-08-19 09:40:03

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

#245 2013-08-19 10:49:27

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

#246 2013-08-19 11:04:09

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

#247 2013-08-19 12:54:55

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

#248 2013-08-19 22:01:39

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

#249 2013-08-20 08:15:25

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

#250 2013-08-20 08:24:37

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

Board footer