roboforum.ru

Собственно, сабж. Есть данные вектор магнитной индукции (от магнетометра) и вектор линейного ускорения (пусть, для простоты, это только гравитация)
Задача - получить кватернион или матрицу ориентации относительно референсной системы координат NED (XYZ = North-East-Down)
Что-то уже перерыл кучу публикаций, нашел много ссылок на некие алгоритмы (QUEST, FQA, ESOQ и другие) но не могу найти реализацию ни одного из них. Буду безмерно благодарен за помощь в данном вопросе.

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

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

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

Добавлено спустя 1 минуту 53 секунды:
https://github.com/dccharacter/AHRS

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

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

Спасибо!

Считать через матрицы - отстой. Через кватернионы правильнее.
Я вот об это мосг себе сломал в свое время: http://diydrones.com/profiles/blogs/dcm ... irst-draft
А кватернионы - перемножил их и усе...

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

Ну на ардуине я юзаю кватернионы для ориентации. Но матрицы- для калибровки сенсоров.
Сделал раширенный фильтр Калмана на ардуину ... и она не потянула. Слишком медленно матрицы умножает- в реальном времени нереал.

stm32f3-discovery. 32бит, 72 мгц, аппаратный fpu, гиро, аксель, компас.
Вот нафиг дуню мучать?

Ну хотя бы потому что она НАМНОГО меньше

размер при желании это поправимо - кроме дискавери полно всяких других плат, да и самому можно сделать

Ребята подскажите. Может кто использовал данный алгоритм.
Хочу сделать 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.

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

для всех данных нужно вычитать 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тьей страницы диалог наш с Дмитрием по поводу нужен компас или нет и если без компаса то какой гиро можно считать "приличным".

Спасибо.

Я в коде одной реализации наткнулся на

Код: Выделить всё

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 писал(а):Зачем данные с гироскопа умножаются на Pi и делятся на 180?

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

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

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

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

https://github.com/dccharacter/AHRS хошь маджвик хошь махони хошь через сериал хошь через юсби
Там же скрипт на питоне для визуализации