roboforum.ru

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

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

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

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

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

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


Уважаемый linvinus, а что здесь смешного? Или вы не согласны со следующим утверждением?
Для того чтобы перевести радианы в градусы воспользуйтесь формулой:

α°= αрад*(180°/π)
Аватара пользователя
lorry
 
Сообщения: 448
Зарегистрирован: 04 фев 2014, 13:53
Откуда: Комсомольск-на-Амуре
прог. языки: машинные коды
ФИО: Иван Иваныч

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

Сообщение linvinus » 23 май 2015, 10:27

По сути вы правы но ваш ответ был не про то.

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


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

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

Сообщение maxbelyx » 19 май 2016, 19:23

Наткнулся на аналогичную проблему. Имею датчик MPU9250, код на Си для Atmega. Считываю данные по шине SPI, хотел бы отфильтровать данные фильтром маджвика, тот, который без компаса.

По началу получал углы с акселерометра вот таким образом: (ax, ay, az - "сырые" данные с датчика)
AccX = atan2(ax, az) * 180/Pi;
AccY = atan2(ay, az) * 180/Pi;

А с гироскопа вот таким: (gx,gy,gz - "сырые" данные с датчика)
GyroX += gx / GYRO_SENS * 0.001; // 0.001 - время опроса датчика, GYRO_SENS = 131 для диапазона 250DPS
GyroY += gy / GYRO_SENS * 0.001;
GyroZ += gz / GYRO_SENS * 0.001;

Угол рысканья по акселерометру, как я понял, получить нельзя. Обрабатывал комплиментарным фильтром по такой формуле:

Угол = 0.98 * GyroN + (1.0 - 0.98) * AccN;

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

Максим.
Последний раз редактировалось maxbelyx 19 май 2016, 20:24, всего редактировалось 1 раз.
maxbelyx
 
Сообщения: 5
Зарегистрирован: 19 май 2016, 19:11

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

Сообщение metric » 19 май 2016, 20:11

У маджвика, если правильно помню, кватернионы используются, а не сырые данные. 9250 может сразу кватернионы отдавать, а не только сырые данные.
metric
 
Сообщения: 158
Зарегистрирован: 23 окт 2015, 19:58

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

Сообщение maxbelyx » 19 май 2016, 20:40

Маджвик работает с квантернионами, а не принимает их, если я верно понял. То, что 9250 может отдавать их, и про DMP знаю, интересует именно то, о чем спросил :)
Максим.
maxbelyx
 
Сообщения: 5
Зарегистрирован: 19 май 2016, 19:11

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

Сообщение linvinus » 20 май 2016, 21:47

В каком виде надо подавать туда эти данные?

данные гироскопа нужно конвертировать в радианы

вот рабочий пример https://github.com/kriswiner/MPU-9250/b ... icAHRS.ino

>MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

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

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

Сообщение Пяткин » 20 май 2016, 22:34

В квартире магнитометр нормально работать не будет
Выводы сделал на основе своего опыта
Пяткин
 
Сообщения: 163
Зарегистрирован: 27 ноя 2013, 15:09

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

Сообщение maxbelyx » 22 май 2016, 05:31

linvinus, спасибо большое! Буду пробовать, посмотрим, что получу на выходе.
Сразу еще вопрос, можно ли эти же данные подать в фильтр маджвика?
Я имею ввиду данные акселя, помноженные на aRes (n/32768), и данные гироскопа помноженные на gRes (n/32768) и переведенные в радианы?
И что Вы имели ввиду под курсом? Если значение по yaw, то оно, на самом деле, не так принципиально, важны лишь данные по pitch и roll. Или они тоже будут плавать, если использовать без магнетометра?
Максим.
maxbelyx
 
Сообщения: 5
Зарегистрирован: 19 май 2016, 19:11

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

Сообщение linvinus » 22 май 2016, 12:13

мадвик или махони большой разницы нет.
данные акселя безразмерные, они нормализуются в алгоритме до юнит вектора

данные гиро обязательно в радианах

да yaw , если yaw не нужен и объект статичен (не находится на быстро движущейся тележке) то аксель вам всегда будет давать точный pitch и roll, если объект движется то гироскоп помогает компенсировать показания акселя, т.к. в этом случае аксель показывает что то среднее между G и вектором движения.

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

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

Сообщение maxbelyx » 24 май 2016, 18:48

Подставил, вроде все работает, спасибо!
Но, заметил что очень долго значения возвращаются в ноль - т.е. делаю резкое воздействие, и ставлю на стол платку, и только спустя секунды 3 оно возвращается в ноль. Поигрался с значениями sampleFreq и betaDef в самом фильтре (использую MadgwickAHRSupdateIMU ), - стало лучше, и даже вроде как удалось подстроить как надо. Но все же, есть какие то правила, по которым надо ориентировать в настройке этих параметров?
Максим.
maxbelyx
 
Сообщения: 5
Зарегистрирован: 19 май 2016, 19:11

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

Сообщение linvinus » 24 май 2016, 21:43

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

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

Сообщение maxbelyx » 24 май 2016, 23:38

На данный момент 50
maxbelyx
 
Сообщения: 5
Зарегистрирован: 19 май 2016, 19:11

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

Сообщение linvinus » 24 май 2016, 23:58

чем выше частота тем луче.

вообще более правильно туда подставлять реально время сколько прошло с момента прошлого запуска MadgwickAHRSupdateIMU

вот код где это используется
Код: Выделить всёРазвернуть
   // Integrate rate of change of quaternion to yield quaternion
   q0 += qDot1 * (1.0f / sampleFreq);
   q1 += qDot2 * (1.0f / sampleFreq);
   q2 += qDot3 * (1.0f / sampleFreq);
   q3 += qDot4 * (1.0f / sampleFreq);


я добавил в функцию ещё один параметр deltat, и получилось так

Код: Выделить всёРазвернуть
   // Integrate rate of change of quaternion to yield quaternion
   q0 += qDot1 * deltat;
   q1 += qDot2 * deltat;
   q2 += qDot3 * deltat;
   q3 += qDot4 * deltat;

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

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

Сообщение grumman351 » 18 сен 2016, 21:32

Подскажите пожалуйста, подставляю в фильтр Маджвика (http://x-io.co.uk/open-source-ahrs-with-x-imu/) данные акселерометра, гироскопа и магнитометра.(mpu6050, lsm303dlhc)

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)

Как бы с бубном не танцевал - получаются пляшущие данные меньше нуля :

Quaternion.jpg


Подставив эти данные в формулу, получаются нули.

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)));

Как должны выглядеть входные данные акселерометра, гироскопа и магнитометра? (у меня с посчитанным смещением)
Как должны выглядеть данные кватернионов из фильтра Маджвика ?

Благодарю.
grumman351
 
Сообщения: 1
Зарегистрирован: 18 сен 2016, 21:10

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

Сообщение linvinus » 19 сен 2016, 00:46

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

Пред.След.

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

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

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

cron