roboforum.ru

Технический форум по робототехнике.


Вычисление кватерниона из данных компаса и акселерометра

Вычисление кватерниона из данных компаса и акселерометра

Сообщение muzhig » 08 авг 2013, 01:03

Собственно, сабж. Есть данные вектор магнитной индукции (от магнетометра) и вектор линейного ускорения (пусть, для простоты, это только гравитация)
Задача - получить кватернион или матрицу ориентации относительно референсной системы координат NED (XYZ = North-East-Down)
Что-то уже перерыл кучу публикаций, нашел много ссылок на некие алгоритмы (QUEST, FQA, ESOQ и другие) но не могу найти реализацию ни одного из них. Буду безмерно благодарен за помощь в данном вопросе.
Аватара пользователя
muzhig
 
Сообщения: 7
Зарегистрирован: 30 апр 2013, 19:48
Откуда: Екатеринбург
Skype: arseny.potapov
прог. языки: python, c++

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение krupoderov » 09 авг 2013, 05:19

Чистая математика с векторами.

1. находишь угол между 2мя векторами гравитации (0,0,1) и твоим с акса (x,y,z)
http://www.pm298.ru/preobr4.php
2. векторное произведение между этими векторами дает тебе перпендикулярный вектор.
3. составляешь матрицу поворота своего вектора относительно перпендикулярного вектора (2) на угол (1)
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0
4. остается только проекция x и y осей (двухмерная система), поворачиваешь этот вектор к вектору магнитометра. грубо говоря вторая матрица поворота отночительно оси Z

в помощь:
http://habrahabr.ru/post/131931/
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0
krupoderov
 
Сообщения: 1
Зарегистрирован: 23 фев 2012, 00:15

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение dccharacter » 09 авг 2013, 09:38

смотри реализации madgwick, mahony

Добавлено спустя 1 минуту 53 секунды:
https://github.com/dccharacter/AHRS
Мой волшебник это я сам. Всю архитектуру программы придумал лично, а ребята помогли воплотить её. Я бы и сам мог написать, но лень учить язык и его конструкции.
Аватара пользователя
dccharacter
 
Сообщения: 4910
Зарегистрирован: 10 дек 2010, 13:16
Откуда: Красногорск МО
прог. языки: C, Python, wiring/processing
ФИО: Андрей

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение muzhig » 28 янв 2014, 23:17

krupoderov писал(а):Чистая математика с векторами.

1. находишь угол между 2мя векторами гравитации (0,0,1) и твоим с акса (x,y,z)
http://www.pm298.ru/preobr4.php
2. векторное произведение между этими векторами дает тебе перпендикулярный вектор.
3. составляешь матрицу поворота своего вектора относительно перпендикулярного вектора (2) на угол (1)
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0
4. остается только проекция x и y осей (двухмерная система), поворачиваешь этот вектор к вектору магнитометра. грубо говоря вторая матрица поворота отночительно оси Z

в помощь:
http://habrahabr.ru/post/131931/
http://ru.wikipedia.org/wiki/%D0%9C%D0% ... 1%82%D0%B0


:ROFL:
Пол года назад прочитал это и ничего не понял. Долго рылся, везде, понял что готовое не найти- кроме готовых реализаций с фильтром вместе. Решил решать задачу сам, подумал и решил - через кватернионы, чуть иначе, но тоже в два поворота. Сейчас натыкаюсь на этот комментарий, читаю его снова и понимаю, что это был правильный ответ. Просто тогда я к нему не был готов :)

Спасибо!
Аватара пользователя
muzhig
 
Сообщения: 7
Зарегистрирован: 30 апр 2013, 19:48
Откуда: Екатеринбург
Skype: arseny.potapov
прог. языки: python, c++

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение dccharacter » 28 янв 2014, 23:21

Считать через матрицы - отстой. Через кватернионы правильнее.
Я вот об это мосг себе сломал в свое время: http://diydrones.com/profiles/blogs/dcm ... irst-draft
А кватернионы - перемножил их и усе...
Мой волшебник это я сам. Всю архитектуру программы придумал лично, а ребята помогли воплотить её. Я бы и сам мог написать, но лень учить язык и его конструкции.
Аватара пользователя
dccharacter
 
Сообщения: 4910
Зарегистрирован: 10 дек 2010, 13:16
Откуда: Красногорск МО
прог. языки: C, Python, wiring/processing
ФИО: Андрей

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение muzhig » 29 янв 2014, 10:41

Ну, я не думаю, что, прямо, отстой. Очень сильно зависит от области применения. Кватернионы лучше матриц только тем что они короче и для их умножения нужно меньше операций. Зато они ограничены единственной трансформацией- поворотом. А матрицы могут объединять в себе сразу много линейных трансформаций- и поворот, и перенос, и наклоны, проекции. И комбинировать можно теми-же самыми умножениями. Кроме того, например OpenGL умеет очень быстро перемножать матрицы. Быстрее чем можно руками кватернионы перемножать. Еще нужно помнить, что для применения поворота в случае с матрицей- мы просто на нее умножаем вектор. А в случае с кватернионами- умножаем кватернион на вектор и на обратный кватернион, который тоже нужно вычислить, а для этого нужно деление и квадратный корень. Так что, количество операций, хоть и меньше, но не всегда быстрее- матрицы обходятся быстрыми операциями сложения и умножения.

Ну на ардуине я юзаю кватернионы для ориентации. Но матрицы- для калибровки сенсоров.
Сделал раширенный фильтр Калмана на ардуину ... и она не потянула. Слишком медленно матрицы умножает- в реальном времени нереал. :)
Аватара пользователя
muzhig
 
Сообщения: 7
Зарегистрирован: 30 апр 2013, 19:48
Откуда: Екатеринбург
Skype: arseny.potapov
прог. языки: python, c++

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение elmot » 29 янв 2014, 14:12

stm32f3-discovery. 32бит, 72 мгц, аппаратный fpu, гиро, аксель, компас.
Вот нафиг дуню мучать?
Аватара пользователя
elmot
 
Сообщения: 5676
Зарегистрирован: 10 ноя 2011, 12:02
Откуда: Turku, Finland
Skype: elmot73
прог. языки: Java и все-все=все
ФИО: Илья

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение muzhig » 05 фев 2014, 15:40

Ну хотя бы потому что она НАМНОГО меньше :)
Аватара пользователя
muzhig
 
Сообщения: 7
Зарегистрирован: 30 апр 2013, 19:48
Откуда: Екатеринбург
Skype: arseny.potapov
прог. языки: python, c++

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение Angel71 » 05 фев 2014, 17:13

размер при желании это поправимо - кроме дискавери полно всяких других плат, да и самому можно сделать
Аватара пользователя
Angel71
 
Сообщения: 9517
Зарегистрирован: 18 апр 2009, 22:18
Предупреждения: -1

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение Dmitry_61 » 20 май 2015, 00:31

Ребята подскажите. Может кто использовал данный алгоритм.
Хочу сделать AHRS(курсовертикаль) для робота, по сути в моём случае мне нужен минимум компас, угол рысканья.
Как считывать данные с сенсоров разобрался.
Собираюсь использовать алгоритм Маджвика(Madgwick algorithm).
В каком виде нужно передать данные с акселя, гироскопа и магнитометра на вход алгоритма?
Какие настройки у алгоритма?
И как получить угол рысканья из выходных данных?

Реализация алгоритма Маджвика на С http://www.x-io.co.uk/res/sw/madgwick_algorithm_c.zip
Чья то реализация на STM32F3 https://github.com/dccharacter/AHRS
Использую сенсоры L3GD20 http://www.st.com/web/en/resource/technical/document/datasheet/DM00036465.pdf
и
LSM303D http://www.st.com/web/en/resource/technical/document/datasheet/DM00057547.pdf
Я делаю на STM32F4, подключены через i2c.
Dmitry_61
 
Сообщения: 5
Зарегистрирован: 09 фев 2015, 00:03
прог. языки: C, C++, C#

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение linvinus » 20 май 2015, 12:57

В каком виде нужно передать данные с акселя, гироскопа и магнитометра на вход алгоритма?

для всех данных нужно вычитать bias (смещение 0),
гиро и аксель калибруются легко, магнетометр сложенее, подробнее тут https://github.com/ptrbrtz/razor-9dof-a ... alibration

Для алгоритма нужно чтобы ось Z была вниз, т.е. микросхема/плата должна быть перевёрнута вверх ногами.

Какие настройки у алгоритма?


Коэффициенты ПД регулятора Kp, Ki и частота вычисления положения по новым данным.

И как получить угол рысканья из выходных данных?


В радианах (yaw - угол рысканья):
Код: Выделить всёРазвернуть
roll  = atan2( 2 *(q0*q1 + q2*q3), pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2));
pitch = asin(-2 *(q1*q3 - q0*q2) );
yaw   = atan2((2*(q1*q2 + q0*q3)),( pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2)));


Но не ждите много от этого датчика и алгоритма, курс будет уплывать.
В моём блоге на первых 2 страницах несколько роликов как я добивался устойчивого курса forum87/topic15337.html

Даже если вы всё откалибруете верно, гиро довольно быстро уплываете, а магнетометр врёт в зависимости от окружающей среды, вот пример forum87/topic15337-15.html#p326155

Начиная с 3тьей страницы диалог наш с Дмитрием по поводу нужен компас или нет и если без компаса то какой гиро можно считать "приличным".
R3 forum87/topic15337.html
R2 roboforum.ru/forum102/topic13980.html
R4 roboforum.ru/post336339.html#p336339
Делать надо хорошо - плохо получится само!
Аватара пользователя
linvinus
 
Сообщения: 1600
Зарегистрирован: 29 апр 2011, 12:37
Откуда: Москва
прог. языки: С

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение Dmitry_61 » 20 май 2015, 23:44

Спасибо.

Я в коде одной реализации наткнулся на
Код: Выделить всёРазвернуть
MadgwickAHRSupdate(val[0]*PI/180.0, val[1]*PI/180.0, val[2]*PI/180.0, val[3], val[4], val[5], val[6], val[7], val[8]);


Зачем данные с гироскопа умножаются на Pi и делятся на 180?
В каком диапазоне должны быть числа входных данных MadgwickAHRSupdate?
Частота обновления как раз важна для вычисления сдвига гироскопа? градусы * время ?
В той же реализации данные с датчиков делятся на какие-либо коэффициенты, я так понял соответствующие чувствительности сенсоров, как их подобрать?
Dmitry_61
 
Сообщения: 5
Зарегистрирован: 09 фев 2015, 00:03
прог. языки: C, C++, C#

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение lorry » 21 май 2015, 00:09

Dmitry_61 писал(а):Зачем данные с гироскопа умножаются на Pi и делятся на 180?

Делить на Pi и умножить на 180, это есть перевод значений угла из радиан в градусы.
Аватара пользователя
lorry
 
Сообщения: 427
Зарегистрирован: 04 фев 2014, 13:53
Откуда: Комсомольск-на-Амуре
прог. языки: машинные коды
ФИО: Иван Иваныч

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение linvinus » 21 май 2015, 00:35

Зачем данные с гироскопа умножаются на Pi и делятся на 180?

Делить на Pi и умножить на 180, это есть перевод значений угла из радиан в градусы.

:ROFL:
http://ru.yasno.tv/article/math/45-ugly ... -i-radiany

В каком диапазоне должны быть числа входных данных MadgwickAHRSupdate?

гиро в радианах, остальное неважно

почитайте что каждый датчик выдаёт на выходе.

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

Я бы рекомендовал начать с готовой программы для ардуино, которая позволяет калибровать и получать результат https://github.com/ptrbrtz/razor-9dof-ahrs

а уж потом с пониманием дела писать своё на STM32

следуйте инструкции по настройке https://github.com/ptrbrtz/razor-9dof-a ... i/Tutorial
R3 forum87/topic15337.html
R2 roboforum.ru/forum102/topic13980.html
R4 roboforum.ru/post336339.html#p336339
Делать надо хорошо - плохо получится само!
Аватара пользователя
linvinus
 
Сообщения: 1600
Зарегистрирован: 29 апр 2011, 12:37
Откуда: Москва
прог. языки: С

Re: Вычисление кватерниона из данных компаса и акселерометра

Сообщение dccharacter » 22 май 2015, 21:13

https://github.com/dccharacter/AHRS хошь маджвик хошь махони хошь через сериал хошь через юсби
Там же скрипт на питоне для визуализации
Мой волшебник это я сам. Всю архитектуру программы придумал лично, а ребята помогли воплотить её. Я бы и сам мог написать, но лень учить язык и его конструкции.
Аватара пользователя
dccharacter
 
Сообщения: 4910
Зарегистрирован: 10 дек 2010, 13:16
Откуда: Красногорск МО
прог. языки: C, Python, wiring/processing
ФИО: Андрей

След.

Вернуться в Сенсорика

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 3

Mail.ru counter