Определение угла наклона акселерометром
Здравствуйте!
Пытаюсь получить углы pitch и roll в формате 0-360гр. от трёхосевого акселерометра по формулам, приведённым на рисунке. XA, YA, ZA данные от акселерометра. Наблюдаю взаимное влияние pitch и roll при переходе 90 и 270 градусов. Моделирование в excel даёт теже результаты. На рисунке виден переворот по roll хотя акселерометр по Y не вращается. Аналогичная ситуация с pitch.
с другими формулами взаимного влияния осей нет, но теряется однозначное определения угла для всего диапазона 0-360гр. (pitch 90гр. = pitch 270гр.)
Подскажите как правильно получить углы?
Здорова, Сань.
Ну, ты даешь… Это ж тригонометрия школьная.
Арктангенс дает значения однозначно из интервала от -90 до 90 градусов. При чем -90 и 90 из отношения YA/ZA не получить. А по этому все распадается на четыре промежутка.
- YA>0, ZA>0
- YA<0, ZA>0
- YA>0, ZA<0
- YA<0, ZA<0
Да и арктангенс этот проще считать таблично. Ты же для микроконтроллера, так?
А для табличного способа еще мельче бить нужно.
1.1 YA>0, ZA>0, |YA|>=|ZA|
1.2 YA>0, ZA>0, |YA|<|ZA|
2.1 YA<0, ZA>0, |YA|>=|ZA|
2.2 YA<0, ZA>0, |YA|<|ZA|
3.1 YA>0, ZA<0, |YA|>=|ZA|
3.2 YA>0, ZA<0, |YA|<|ZA|
4.1 YA<0, ZA<0, |YA|>=|ZA|
4.2 YA<0, ZA<0, |YA|<|ZA|
И для каждого промежутка вычислять отношения: tg(a) = ZA/YA при |YA|>=|ZA|, и tg(b) = YA/ZA при |YA|<|ZA| из которых по таблице определять угол, соответствующий каждой из 8 частей.
А второй схемкой вообще умилил. Значение считаешь как для Arcsin(x), а из него берешь Arctg(x)… 😃
только не понятно, зачем для этого использовать аксель, если давным-давно придумали 3х-осевой ДУС? там и скорость квантования высоченная, и выход SPI, разрешение 10 или 12 бит. простейший интегратор на контроллере спасёт мир!
только не понятно, зачем для этого использовать аксель, если давным-давно придумали 3х-осевой ДУС?
ДУС дает большие погрешности при выработке параметров ориентации. (крен/тангаж) Нужна компенсация от акселерометра. Мы тут с Санькой не то чтобы “забились”, но все-таки решили идти параллельными курсами к компенсируемому мемс гироскопу на основе одной и той же элементной базы. Он начал с акселерометра, я – с ДУСа (гироскопа). Обмениваясь информацией, надеюсь, совместно придем к чему-то. 😃
простейший интегратор на контроллере спасёт мир!
Нет. Не спасет.
Там идет постоянная составляющая от АЦП, которая зависит от множества параметров. Напрямую ее отфильтровать и компенсировать невозможно.
Я сделал компенсацию по компасу, но это довольно сложный алгоритм… Именно по этому большенство народа для определения горизонта используют аксели. Аксели хороши тем, что в принципе это как стакан с водой. Пока летишь ровно, вода в стакане показывает и крен и тангаж. А как начинаешь маневрировать, вот тут все и начинается. 😃
И еще. Прямое интегрирование в принципе не возможно. Нужно интегрировать положение 3-х осей после каждой иттерации. А это ого-го сколько вычислений в плавучке. 😃
А это ого-го сколько вычислений в плавучке.
Cortex с FPU спасёт мир:)
Обмениваясь информацией, надеюсь, совместно придем к чему-то.
Михаил, а может здесь тоже будете выкладывать? Тема-то достаточно актуальная, в т.ч. и для меня:)
Использую cortex m3, пишу на C, стандартная математическая библиотека math.h. Для atan2 знаки обоих аргументов используются для вычисления квадранта результата.
Михаил, ты так предлагаешь?
void to_angle(void){
if (fYA>0|fZA>0){
roll = atan(fYA/fZA);
return;
}
if (fYA>0|fZA<0){
roll = atan(fZA/fYA);
return;
}
if (fYA<0|fZA>0){
roll = -atan(fYA/fZA);
return;
}
if (fYA<0|fZA<0){
roll = -atan(fZA/fYA);
return;
}
}
Использую cortex m3
чей кортекс, если не секрет? а к m4 вроде как добавили FPU как отдельный сопроцессор плавающей запятой.
Александр, roll - это глобальная переменная, кот. где-то потом используется? не очень ясен смысл ф-ии… всмысле матем-ий ясен, а практический не очень.
Если у тебя Арктангенс2 такой умный, можешь попробовать:
roll = atan2(-ZA,YA);
pitch= atan2(-ZA,-XA);
Это я пишу исходя из осей акселерометра. Они не совпадают с принятыми осями в навигации. Там по-другому.
А если только тангенс, то
void to_angle(void)
{
if ((fYA<=0)&&(fZA>0))
{
roll = atan(-fYA/fZA) - pi();
return;
}
if (fZA<0)
{
roll = atan(-fYA/fZA);
return;
}
if (fYA>=0)&&(fZA>0)
{
roll = atan(-fYA/fZA) + pi();
return;
}
if (fYA<0)&&(fZA=0)
{
roll = -pi()/2;
return;
}
if (fYA>0)&&(fZA=0)
{
roll = pi()/2;
return;
}
}
для Петруччо: LPC1768, STM32F100C4T6B. Мощи много после AVR. FPU для этого проекта вряд-ли нужен будет. roll глобальная, используется пока только для визуализации угла.
в ответ на #9
в обеих случаях работает если не крутить по оси X, иначе перевороты в районе ±90гр как на первом рисунке #1
в обеих случаях работает если не крутить по оси X, иначе перевороты в районе ±90гр как на первом рисунке #1
Там все верно. “Переворот” по тангажу – это правильно. Вот гляди, два случая:
- Летим в прямом полете. Тангаж +2 градуса. Крен 1 градус. Начинаем вертеться по крену не меняя тангажа. Получается, что тангаж будет положительный. И так мы крутимся до крена 89 градусов. Все вроде бы правильно.
- Летим в перевернутом положении. Тангаж, при той же проекции силы тяжести на ось X, уже будет не +2, а -2. Соответственно и по крену тоже показания. Будет не +1, а +179. Начинаем вертеться по крену от 179 до 91 градуса. И тут тоже тангаж остается отрицательным. Как и положено.
А теперь посмотри в каком положении самолет при тангаже +2, крене 89, и тангаже -2, крене 91? Да это же почти одно и то же. Отличие на 2 градуса по крену.
Вот и получается, что в 90 градусов по крену тангаж меняет знак. Он начинает показывать переход от прямого к перевернутому полету.
(Сорри. Может, конечно, немного попутал. И по крену тоже углы будут менять знак, но принцип понятен.)
Относительно меня или относительно вас 😃
Я стою на земле, смотрю на самолёт Михаила, и вижу, что он делает бочку, никакого переворота по тангажу не вижу, если- бы увидел как хвост поменялся с носом в момент перехода по крену 90гр. очень бы удивился 😃
Относительно меня или относительно вас 😃
Относительно того, что измеряешь. Можешь, конечно аксель и к земле прикрутить, а не к модели. Дело хозяйское.
Я стою на земле, смотрю на самолёт Михаила, и вижу, что он делает бочку, никакого переворота по тангажу не вижу
Вот. Красным обозначены оси акселерометра (X, Z). Черным – горизонтальная ось земли (Xз). Сила тяжести - F.
Если вертеть вдоль оси Х, то видно, что в нормальном полете угол между X и Хз - положительный. В перевернутом – отрицательный. Т.к. относительно перевернутой оси Z, связанной с моделью, модель зарывается как бы ниже оси Хз.
Михаил, ты поворачиваешь систему отсчёта вместе с самолётом, я же хочу получить углы относительно земли.
я же хочу получить углы относительно земли.
А смысл? Захотелось посвязываться с кватернионами? (от одного названия волосы дыбом) На самом деле углы относительно земли, на мой взгляд, достаточно бесполезны. Может я чего-то не вижу?? В любом случае, данные нужны для того, чтобы удерживать модель на определенном крене/тангаже. Так? И в этом случае автопилот будет работать в каком-то ограниченном пределе их изменения, думаю не более чем +/-45 градусов по тангажу и +/-30 по крену. Т.е. НЕ автоматизировать перевернутый полет. Разве что определить, что модель летит кверхтормашками и попытаться ее вывести в нормальный режим полета. А в нормальном режиме полета указания по крену и тангажу будут совпадать с наблюдаемыми с земли.
Михаил, ты поворачиваешь систему отсчёта вместе с самолётом
Я ее не поворачиваю. Она в микросхеме акселерометра жестко прошита, и будет крутиться вместе с микросхемой.
Кстати, как вариант, можешь проверку на знак fZA сделать. И если fZA>0, то в расчете тангажа подставлять (-fZA).
Я сделал компенсацию по компасу, но это довольно сложный алгоритм… Именно по этому большенство народа для определения горизонта используют аксели.
code.google.com/p/imumargalgorithm30042010sohm/ 2010 год, MARG на кватернионах. Гарантировано работает, сам проверял - крутить можно вверх ногами, вращать на ребре и т.д. Повторяемость углов положения поразительная. Коррекция ДУС по магнетометру все 3 оси, по акселерометру все 3 оси. В конце кода есть небольшая ошибочка с присвоениями =)
На базе этого алгоритма построена библиотечка www.varesano.net/projects/hardware/FreeIMU, но в ней есть косяк, который дает дрифт Yaw. Эту библиотеку удобно использовать для опроса датчиков, а МАРГ переписать. После этого получается конфетка весом около 20 кб, которая дает кватернион/готовые углы/сырье.
Гарантировано работает, сам проверял - крутить можно вверх ногами, вращать на ребре и т.д. Повторяемость углов положения поразительная. Коррекция ДУС по магнетометру все 3 оси, по акселерометру все 3 оси. В конце кода есть небольшая ошибочка с присвоениями =)
А как себя этот фильтр ведет, если нет данных магнитометра?
И (по крайней мере на первый взгляд на код) ошибок не нашел.
Эту библиотеку удобно использовать для опроса датчиков, а МАРГ переписать. После этого получается конфетка весом около 20 кб, которая дает кватернион/готовые углы/сырье
Со второй частью что-то совсем не понял, где там искать библиотеку?..
А как себя этот фильтр ведет, если нет данных магнитометра?
И (по крайней мере на первый взгляд на код) ошибок не нашел.Со второй частью что-то совсем не понял, где там искать библиотеку?..
под версиями железа “FreeIMU library”
www.varesano.net/…/libraries_20120118_0959.zip
А как себя этот фильтр ведет, если нет данных магнитометра?
- mx=0; my=0; mz=0; Коррекция ДУС только по акселерометру. Работает, но сильнее “ловит” линейные ускорения и дает несуществующие наклоны.
- ax=0; ay=0; az=0; Коррекция ДУС только по магнетометру. Работает, но заторможеннее, чем в комплексе с акселерометром.
- mx=0; my=0; mz=0; ax=0; ay=0;az=0; Коррекции ДУС не происходит. Работает, но по курсу плывет.
Опаснее критические значения с датчиков, а не нулевые.
Нужно как-то “ловить” опасные показания, например, когда магнетометр находится в сильном ЭМ поле, то коррекция по компасу приведет к аварии. Можно даже “раскрутить” ИМУ по любой оси, действуя на магнетометр. Аналогично, акселерометр нужно фильтровать, отсекать “глюки” неверной калибровки диапазона, к примеру.