Библиотеки для инерциальной навигации - какие есть?

Чтобы не париться с объединением результатов акселерометра, гироскопа и прочих? Вдруг есть готовое решение?

Всё в 1 флаконе чипе TIMU.

Существование универсальной библиотеки маловероятно, ибо точность вычислений, зависящая от привязки к аппаратной части, важнее универсальности.

Ну так это TIMU будет выдавать максимально точные актуальные 100% готовые данные? Чтобы я взял платку с этим TIMU, крутил как бешеный, а потом положил туда же и вижу ноль?

Меня интересует максимально готовое решение.

Некоторые вон делают акселерометры и прочее с аналоговыми выходами, а ты цифруй и фильтруй как умеешь. У меня попроще - выдает по I2C оба датчика, но ловить по прерываниям надо и комбинировать чтобы получить реальное положение.

В общем, не 100% готовое решение...

Это TIMU разумеется мне не подходит, ибо недоставабельно. А что есть еще из готового?

Ты бы хоть сказал, под какую платформу это завести хочешь. И, кстати, откуда бы взялись "универсальные решения", если возможностей реализации — тьма тьмущая!

Ты можешь подключить свои акселерометры к конвертеру I2C/RS-232, а потом чисто компьютером анализировать; можешь выполнять некий предварительный анализ при помощи микроконтроллеров; можешь вообще по GPIO к какой-нибудь "малинке" подключить. И?

У меня сейчас уже в наличии данные с гироскопа (с частотой 1 КГц) + значения акселерометра. Хочу теперь взять начальное положение за ноль, подергать платку датчиков и положить обратно и увидеть ноль, т.е. отследить все перемещения, правильно их обсчитать.

Надо писать код - разумеется. А вдруг есть готовый параметризуемый?

Тьфу ты! До меня дошло, зачем ты хочешь данные с акселерометра дублировать данными с гироскопа (а то ведь при ускорениях/торможениях получится хрен-те что)!

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

В общем, без опорных базовых станций задача нерешаема!

В моей задаче они есть (разумеется), но я о них скромно умолчу.

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

Пока что первое что мне нашлось это примитивный комплементарный фильтр и я надеюсь мне его хватит...

Погугли "кватернионы". Очень пригодится.

Меня интересует максимально готовое решение.

Есть в любом, ближайшем к тебе, НИИ/заводике, выпускающем изделия с гиростабилизированной платформой.

Это TIMU разумеется мне не подходит, ибо недоставабельно. А что есть еще из готового?

Модули для самоделок: Accelerometers, Gyros, & Compasses.

Нуль я не получу, будет дрейф.

Ага. И да поможет тебе фильтр Калмана, аминь! :)

ФК я знаю, но блин, я не понимаю этот фильтр. Ну Калман молодец, я не могу применить эти знания на практике, не выходит.

не понимаю этот фильтр... не могу применить эти знания на практике, не выходит.

The Kalman Filter — куча полезного материала, включая Software (внизу).

Тебе надо гуглить реализацию фильтра Калмана для этой задачи. Из готового посмотри какие-нибудь открытые проекты квадрокоптеров, где гироскоп с акселерометром параллельно используются. ----- угу, тред не читай - сразу отвечай

Там нашел: http://bayesclasses.sourceforge.net/Bayes .html и http://www.orocos.org/bfl - спасибо! Уже что-то, явно готовое и просто параметры задать.

За pololu спасибо - самый дешевый с гироскопом - 40 баксов, а мне много таких надо. Есть ли еще дешевле с приемлемыми параметрами?

Сразу мысль - а может свою платку развести? Так будет дешевле?

самый дешевый с гироскопом - 40 баксов
Есть ли еще дешевле с приемлемыми параметрами?

Есть дешёвые модули для Arduino, но параметры там для игрушек-самоделок: GY-80 BMP085, MPU-6050, ITG-3200, ITG3205.

Сразу мысль - а может свою платку развести? Так будет дешевле?

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

Слушай... Так у меня как раз ITG-3200 и я мучаюсь с этим говном...

Неужели это паршивый модуль гироскопа? Если купить другой - это уже серьезнее будет? С более приличными параметрами - намного дороже? Можешь численно озвучить параметры?

С более приличными параметрами - намного дороже?

Да, на 2 десятичных порядка при переходе с 16 на 24 бита точности.

Можешь численно озвучить параметры?

Не могу, ибо не знаю на какую разведку ты работаешь? :)

Но могу другой промышленный пример: STIM300.

У меня сейчас уже в наличии данные с гироскопа (с частотой 1 КГц) +
значения акселерометра. Хочу теперь взять начальное положение за ноль,
подергать платку датчиков и положить обратно и увидеть ноль, т.е.
отследить все перемещения, правильно их обсчитать.

Попробуй вращение по одной оси (закрепи датчик где нибудь) проинтегрировать, простое сложение. А потом расскажи какой там ноль получается.

STIM300

Спасибо, только что выяснил он 10К евро стоит, не надо )))

Ну а что-нибудь за $100? Есть же нечто среднее между дешевым гогном и супер-дорогими хреновинами?

И получается! В течение минуты более менее ноль видать, а вот через минут 5 уже несколько уезжает. Я его беру со стола, верчу, кладу обратно - почти ноль. Стабильно в течение 30 секунд - уже меня устраивает.

Суть лишь в объединении результатов акселерометра и гироскопа чтобы просто увидеть реальное положение объекта. И всё.

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

Переживает быстрые повороты? Итеграл в результате возвращвется в ноль? Сколько точно?

Ну, по моей информации, классический (пусть даже упрощенный) БИНС на дешевых mems не получается. Без коррекции разваливается за секунды, а хоть как-то стабилен только в покое. Хотя у меня пока нет времени попробовать самому. Поэтому и спрашиваю.

Я потом скажу точнее, но за пол минуты уползает на градус точно - покрутил покрутил, положил на место - вижу градус уехал...

Если повернуть на 90, покажет 90+-1?

Да, хорошо покажет. Главное правильно результаты умножать - учитывать дельту времени и всё такое. И тогда будет как надо. Тем более если просто повернуть.

Если использовать коррекцию акселерометра по гироскопу, то точность определения угла получается +-1 градус. Это что касается угла поворота по X и Y (для Z нужен компас, а его у меня пока нет). А вот что будет с координатами не скажу, пока не пробовал такое реализовывать. Железо - микроконтроллер AVR + китайская платка с MPU-6050. В общем, не так уж и плохо всё.

P.S.: Точность определения угла, разумеется, падает при длительном ускорении, но после того, как оно пропадёт всё нормализуется. Если кому надо, могу скинуть фрагмент моего быдлокода.

Какой софт ты брал для совмещения гироскопа и акселерометра? Статью можешь посоветовать? Какая самая понятная и помогла решить задачу?

Если кому надо, могу скинуть фрагмент моего быдлокода.

ДА!!! И вопросы, если можно задам малость по коду...

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

Одно уравнение Пуассона в связанной системе координат, на входе измерения дус, на выхоже дает ориентацию. Имея ориентацию, акселерометры пересчитываются в неподвижную систему координат, где интергрируются раздельно по осям, гравитация вычитается.

Добавка: Забыл спросить про точность акселерометра, если его поинтегрировать и пошевелить, так же хорошо?

Если использовать коррекцию акселерометра по гироскопу

Не гироскопа по акселерометру? И это уже коррекция c предположениями о характере движения, а не ИНС.

Мне бы для начала - просто понять как уточнять гироскоп по акселерометру... Просто формулу, простейшее...

Перед тем как уточнять, ориентацию все таки надо получить, интегрируя уравнения и давая им на вход угловую скорость.

Самое простое это работать с матрицами вращения, наверно вот здесь об этом проще всего писано gentlenav.googlecode.com/files/DCMDraft2.pdf

void sensors_init() {
	_delay_ms(20);
	mpu6050_write(MPU6050_PWR_MGMT_1, MPU6050_CLKSEL_INTERNAL);
	_delay_ms(10);
	mpu6050_write(MPU6050_SMPLRT_DIV, 0);
	mpu6050_write(MPU6050_CONFIG, MPU6050_EXT_SYNC_DISABLED | MPU6050_DLPF_44HZ);
	mpu6050_write(MPU6050_GYRO_CONFIG, MPU6050_FS_SEL_500);
	mpu6050_write(MPU6050_ACCEL_CONFIG, MPU6050_AFS_SEL_4G);
	_delay_ms(10);
	sensors_read(true);
}

float normalize_angle(float angle) {
	if (angle > M_PI) {
		angle = -M_PI + (angle - M_PI);
	} else if (angle < -M_PI) {
		angle = M_PI + (angle + M_PI);
	}
	return angle;
}

void sensors_read(bool first) {
	sensors_need_read = false;
	sensors_read_count++;
	// Read data
	temperature = mpu6050_read_data(MPU6050_TEMP_OUT_H) / 340.0 + 36.53;
	float gyro_dx = (float)mpu6050_read_data(MPU6050_GYRO_XOUT_H) / 65.5 / 180.0 * M_PI;
	float gyro_dy = (float)mpu6050_read_data(MPU6050_GYRO_YOUT_H) / 65.5 / 180.0 * M_PI;
	float gyro_dz = (float)mpu6050_read_data(MPU6050_GYRO_ZOUT_H) / 65.5 / 180.0 * M_PI;
	accel_x = (float)mpu6050_read_data(MPU6050_ACCEL_XOUT_H) / 8192.0;
	accel_y = (float)mpu6050_read_data(MPU6050_ACCEL_YOUT_H) / 8192.0;
	accel_z = (float)mpu6050_read_data(MPU6050_ACCEL_ZOUT_H) / 8192.0;
	// Calculate angle by accelerometer
	float accel_rx = atan(accel_x / sqrt(accel_y * accel_y + accel_z * accel_z));
	float accel_ry = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z));
	// Check  NaN and INF
	if ((accel_rx != accel_rx) || (accel_rx == INFINITY) || (accel_rx == -INFINITY)) accel_rx = gyro_x;
	if ((accel_ry != accel_ry) || (accel_ry == INFINITY) || (accel_ry == -INFINITY)) accel_ry = gyro_y;
	// Appy gyroscope value to current angle
	if (first) {
		gyro_x = accel_rx;
		gyro_y = accel_ry;
		gyro_z = 0.0;
	} else {
		gyro_x += gyro_dx / SENSORS_FREQ;
		gyro_y += gyro_dy / SENSORS_FREQ;
		gyro_z += gyro_dz / SENSORS_FREQ;
		gyro_x = normalize_angle(gyro_x);
		gyro_y = normalize_angle(gyro_y);
		gyro_z = normalize_angle(gyro_z);
	}
	// Filter angle
	gyro_x = 0.9 * gyro_x + 0.1 * accel_rx;
	gyro_y = 0.9 * gyro_y + 0.1 * accel_ry;
}

Алгоритм может и примитивный, но он точно работает. Если ускорение не большое, то точность +-1 градус и никуда не уплывает (просто может колебаться чуть-чуть на одном месте в пределах этого диапазона), если есть большое ускорение точность падает, но лишь пока оно не прекратиться.

У кода есть один минус - он сходит с ума, если угол наклона более +-90 градусов (то есть плату совсем перевернули к верх ногами) и выдаёт бред, пока угол не станет меньше +-90 градусов. Буду признателен, если мне укажут как его доработать.

Да, ошибся. Конечно же, уточнение гироскопа по акселерометру.

Для ROS есть готовые пакеты. Но без фузион с данными камеры сколько нибудь стабильная навигация невозможна.

Я прочитал несколько магистерских работ где студиозусы пытались сделать чистую инерционку из г. и п. Результат весьма слабый.

Инерционка дает точные и быстрые данные для очень коротких отрезков времени и малых смещений. А навигация по картинке с камеры объекта (модуль называется PTAM) наоборот --- для длительных по времени и больших по расстоянию. Есть кандидатская чувака который напилил модуль для ROS который синтезирует эту инфу. Модуль средней готовности. Но мне кажется --- тебе PTAM хватит за глаза :)

gyro_x += gyro_dx / SENSORS_FREQ;
gyro_y += gyro_dy / SENSORS_FREQ;
gyro_z += gyro_dz / SENSORS_FREQ;

Рукалицо.

Что не так? И да, я предупреждал, что качество может быть не очень. Жду замечаний, что можно улучшить)

1. Уравнения в углах интегрируются гораздо сложнее, это не просто раздельные суммы. Реализация не корректна.

2. Линейная фильтрация углов тоже скорее всего не корректна.

3. Выбор углов для представления ориентации, самый неудачный выбор. Углы можно получить на выходе если они нужны, но не делать интегрирование в углах.

Чисто по изображению с камеры ищет координаты??? Спасибо почитаю, там наверное какие-то маячки есть светодиодные...

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

Ответ на вопрос в треде уже есть.

схема, обзорная ссылка

quickquest Anon prischeyadro amaora KivApple psv1967

Предлагаю ознакомиться с моими данными. Тут я просто поднял датчики, повертел их, положил на стол в том же месте и положении (в идеале должно вернуться в ноль): http://pastebin.com/a1Gg5XFk

# gyro+acc          6 skips=  80
G    -342    -579       5
A     132      -3      -8

Значения гироскопа считаются так:

float sf = (float) 0.02 / 14.375; // 20 ms, и еще коэф. из доков
f_xyz[0] += xyz[0] * sf;
f_xyz[1] += xyz[1] * sf;
f_xyz[2] += xyz[2] * sf;

Как теперь из этого получить координаты? Чтобы при визуализации я увидел что поднялась платка, крутилась, двигаласть вверх-низ и легла обратно?

градусы.
Как теперь из этого получить координаты?

Это называется «переход из сферической в декартову систему координат». http://ru.wikipedia.org/wiki/Сферическая_система_координат

Потратил несколькок часов на этот набросок в octave.

http://paste.org.ru/?h05ldm

http://postimg.org/image/qw8d5t3zf/ http://postimg.org/image/lr6x8ebo7/

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

Исходные данные надо давать в виде «плоской» таблицы, одинт такт - одна строка значений, без разметки, так проще обрабатывать.

Чтобы я взял платку с этим TIMU, крутил как бешеный, а потом положил туда же и вижу ноль?

Ошибка измерений + Ошибка вычислений и фиг тебе а не ноль. Чем дольше вертишь тем больше будет у тебя уход от нуля. И хоть ты тут лопни.

Исправил жуков, добавил простой фильтр сбавляющий скорость (но это уже не ИНС).

http://paste.org.ru/?6m8utt

http://postimg.org/image/r4ksydmj9/ http://postimg.org/image/fh4wmzd8f/

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

Спасибо! Это в Octave?

Учту... я не думал что кто-то захочет прогнать эти данные, завтра выложу в формате попроще. В каком лучше? Мне в любом случае надо будет продолжать работу...

Я кстати обнаружил в своей программе в которую подапал данные что в ноль не выходит, возможно параметр в датчик какой-то неправильно засел, завтра тщательно перепроверю. А ведь раньше выходило после любых поворотов.

никаких скрипачей, ой маячков :) просто картинка с _одиночной_ камеры! PTAM очень мощная вещь.

Нашел PDFку (KleinMurray2007ISMAR.pdf), почитал, там же внутри фотки - в самом деле так и есть. Однако мне это не подходит, честно. Но для других задач - интересно будет применить PTAM.

ЗЫ Видал чудеса техники, но такооого :)

Так и есть... неверные данные. На некоторых замерах правильные и я вижу корректные углы в конце, а во многих - нет. Буду разбираться и потом выложу проверенные данные. В которых углы соответствуют проведенным движениям.

А потом уже сращивать с акселерометром надо будет...