Search in topic

Создание собственной системы стабилизации

Угол_гир_х= Угол_гир_х + данные_гир_х *0.00008;

а что это?
я так понимаю интегрирование должно быть, тогда что такое 0.00008 - яко бы время между отсчётами? но оно может быть разным…

ИИ здесь вреден. Интеллект и так есть - это интеллект пилота.

не, не вреден, только в данном узле он не нужен )))

а что это?
я так понимаю интегрирование должно быть, тогда что такое 0.00008 - яко бы время между отсчётами? но оно может быть разным…

Время одинаковое. В секунду у меня проходит 380 считываний датчика. И в зависимости от диапазона измерений гиры это число будет разное.
Проделал эксперимент. Плата на столе. Поднимаю и медленно вращаю по х на 360 градусов , кладу. Насчитал 365. Далее в обратную сторону и на стол , угол 1 гр.
И за 4 полных таких цикла угол 0 градусов. Коррекции по акселю убрал.

Время одинаковое. В секунду у меня проходит 380 считываний датчика.

в поликлинике тоже может быть средняя температура 36,6 но у одних она еще 40 а у других комнатная.
нужно брать реальное дельта времени за период измерения а не среднее за секунду

в поликлинике тоже может быть средняя температура 36,6 но у одних она еще 40 а у других комнатная.
нужно брать реальное дельта времени за период измерения а не среднее за секунду

Это не влияет абсолютно на результат. Есть у кого конкретные примеры со
своего железа.

Угол_гир_х= Угол_гир_х + данные_гир_х *0.00008;
Угол_гир_y= Угол_гир_y + ( данные_гир_z*0.02 + данные_гир_y) * 0.00008;

Методический косяк: линейное интегрирование каждой оси в отдельности правильной ориентации в 3х мерном пространстве не даст никогда! Ну разве что в случае вращения идеально вокруг одной оси. Интегрирование необходимо выполнять через матрицу поворота или кватернион.

Ошибки нет. Также есть влияние оси z на y. Заметил что коптер при развороте вокруг оси уносит в сторону. Положил плату на стол. Углы по акселю и гирам 0.

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

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

Ошибочное мнение. Коптер отлично держит горизонт, получше всяких готовых прошивок . Могу продемонстрировать , живем в одном городе.
Для полета задаем угол по оси , с пульта. Коптер наклоняется и автоматом добавляет газ обратно пропорционально COS угла наклона.

Доб_газ= 70/(COS(угол_наклона)) -70 , например при 30 град. добавляем 11 единиц шим

Также есть автомат увеличения газа по измерению напряжения батареи , при снижении напряжения на 2 V добавляется примерно 20 единиц. У меня был тока один недочет. При горизонтальном полете ,когда сбрасывал угол , коптер уходил вверх метров на 5-7. На регуляторах стояла прошивка с неработающим торможением Damped и при выравнивании коптера 1-1.5 сек оставались повышенные обороты, больше чем надо для висения.
Прошил более старой версией и торможение заработало. Пока не проверил , погода у нас фиговая. Ну а вообще управление на 3DR Radio модемах.
Пульт свой( модернизированная Turnigy 9x ), протокол передачи свой. На пульт приходит информация с коптера ( GPS данные, напряжение батареи) и выводится на олед дисплей 20х4 .
При разряде батареи на коптере , в пульте пищит зумер.

Но стоит наклонять плату постепенно
меняя ось , как бы по сфере но без вращения по оси z , то гиры начинают врать. Угол уходит прилично на 10-20 град. Это влияние осей друг на друга ?

А мне кажется, что ошибка при медленном вращении платы это результат банального шума гиры (так и должно быть), а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ… (А формула связывающая разные оси через линейный коэф. приведет к непредсказуемым результатам на разных режимах полета…)

а вот как раз коррекция по акселю для устранения этого шума и нужна, НО не раз в пять секунд !, а каждую итерацию с частотой расчета ИМУ…

Так и происходит формула :
угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001
Каждое измерение приближает значение угла гиры к углу акселя. Если у меня 380 измерений в сек. то для коррекции за 3 сек надо коэф. 1/(3*380 ) примерно.
Никакого шума гир нет. Я калибрую их при включении питания. Делаю 4000 измерений , нахожу среднее для каждой оси , и этот коэф. вычитаю при каждом считывании данных.
Тут все просто. Если мы по Х наклоним на 45 градусов , а потом по Y на 45гр.( относительно оси X ) то для оси земли это уже не 45 гр. Более лучше это понять если наклоним в право или лево на 90 градусов. Тогда ось Y вообще теряет смысл как ось , которая показывает наклон относительно земли., она превращается в Z. Но для отличной стабилизации хватает по PID регулятору на каждую ось.

угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001

Всё правильно, если так, и больше ничего не нужно… (старый добрый комплементарный фильтр") ищите косяк в другом месте… начиная с “сырых” данных датчиков и до выходных показаний PIDa…

Так уже сказали где косяк:

Так и происходит формула : угол_гир_х = угол_гир_х * 0.999 + угол_акселя_х * 0.001

• нельзя интегрировать гироскоп раздельно по осям. К примеру, наклони его на 90 вбок, поверни по азимуту на 90 и верни обратно. Проинтегрировался угол по одной оси, а реально сенсор развернулся по другой.

и больше ничего не нужно…

Да, еще матрицы или кватернионы естественно… ))

Спасает то что летаем при небольшом угле , до 30 градусов и погрешность при таком алгоритме небольшая.
И в основном летают одной осью вперед ,на которой камера.
Можно прикинуть допустимый угол наклона для полета.

488Гц частота шим ардуины ,и максимальное время шим 2049 мкс.

2049/255=8.035 мкс один шаг. шима

Регулятор можно запрограмировать на диапазон от 1000 до 2020 мкс.

Тогда минимальное значение шим для регулятора 1000/8.035=124 ед , даем при сетапе

analogWrite( 9 , 124);

Вот тут могут быть проблемы. 1000мкс при программировании регулятора могут не совпадать с нашими 1000 в ардуине.
Могут отличатся немного частоты кварца, и не все регуляторы инициализируются.
Нужно менять число на другое 123 или 125 к примеру. ( калибровка от пульта у меня отключена )

Максимальный уровень шим 2020/8.035=251 ед
Если будем давать больше 251 , регулятор не воспримет этот диапазом, а иногда даже может быть провал оборотов.

Ограничиваем диапазон шим на регулятор

M1=constrain( gaz + DM1a + DRZ + DRH , 124 , 251);
analogWrite( 9 , M1 );

Висим на 50 % газа , это 124+ (251-124)/2=188 ед

При проседании питания на 2V добавить надо 20 ед.
Еще запас на диапазон регулировок PID регуляторов -20 +20

Получаем 251-188-20-20=23 ед. Это число шим которое пойдет на коррекцию газа при наклоне.

Тяга_при_наклоне = Тяга_висения/cos(угла)

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

шим_коррекции = 70/Cos(угол)-70

отсюда при максимальной коррекции в 23 ед угол может быть 41 градус

Ошибочное мнение. Коптер отлично держит горизонт, получше всяких готовых прошивок .

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

Вот собственно ответ, почему летает - коррекция от акселя перетягивает интегральную ошибку.
Предлагаю немного подкорректировать Ваши формулы, Сергей, и получить “дубовый” (в хорошем смысле слова) алгоритм, но БЕЗ ОПРЕДЕЛЕНИЯ ОРИЕНТАЦИИ. Предполагаю, судя по поведению, аналогичный алгоритм используется в китайских игрушках.
Делаем так:

Сумм_крен +=ось_акселя_y * Кки;
Сумм_танг += ось_акселя_x * Кти;
шим_крен= угол_гир_х * Kкд + ось_акселя_y * Kкп +Сумм_крен + ось_РУ_КРЕН*Ккру;
шим_танг= угол_гир_y * Ктд + ось_акселя_x * Kтп+Сумм_танг + ось_РУ_ТАНГ*Ктру;
шим_рыск= угол_гир_z * Крд + ось_РУ_рыск*КРру;;
шим_газ = шим_газ* 0.999 + (ось_акселя_z- 1) * 0.001 + ось_РУ_рыск*Кгаз_ру;;

Надо только нормировать аксель до 1 (единица =1G), либо можно всё делать в целочисленном формате. И работать будет на “ура” даже на MCS-51.
Осталось только смешать это всё в сумматоре, для конфигурации “+” будет выглядеть вот так:

мотор_перед = -шим_танг+ шим_газ;
мотор_задний = шим_танг+ шим_газ;

мотор_левый = -шим_крен+ шим_газ;
мотор_правый = шим_крен+ шим_газ;

Летать будет как по рельсам.

Если интересно, могу дорисовать компас.

мотор_левый = -шим_крен+ шим_газ

некатит. нужно проверять нет ли лимитов по шиму вывоимому на мотор и если есть то в зависимости от приоритетов (удержание левела - удержание высоты - удержание курса) к примеру если сумма пвм на мотор больше максимума по калибровке то нужно вычесть из опозиционного мотора избыток

при калькуляции шима нужно учитывать компенсацию моментов вращения - иначе не избежать раскачки

некатит. нужно проверять нет ли лимитов по шиму вывоимому на мотор и если есть то в зависимости от приоритетов (удержание левела - удержание высоты - удержание курса) к примеру если сумма пвм на мотор больше максимума по калибровке то нужно вычесть из опозиционного мотора избыток
при калькуляции шима нужно учитывать компенсацию моментов вращения - иначе не избежать раскачки

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

1. каждый ПИД должен иметь отдельно ограничения ПД и И-составляющих.
2. Чтоб избежать переполнения по общему газу, его тоже надо ограничить до 0.85-0.9 от макс_шим (у меня 0.85).

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

И ещё не совсем понял про приоритеты, тут вроде всё просто: главная задача обеспечивать угловую стабилизацию, а дальше уже всё остальное. В идеале газ висения должен быть 1/2 от макс_шим, поэтому вся система стабилизации, в т.ч. и удержание высоты будет работать, с достаточным запасом по динамике. Иначе нужно менять конфигурацию коптера (масса и ВМГ).

Идеально сбалансированный коптер будет иметь в висе

Сумм_крен= 0,
Сумм_танг = 0,
шим_газ = 0.5

.

И ещё нужно чтоб аксель был калиброван в хотя бы в горизонте, т.е. при установке на гризонтальную плоскость показывать

x=0,
y=0,
z=1

А ДУС (гира) в покое давать нули. У меня смещения ДУС вычисляются автоматически непосредственно перед полетом между армингом и запуском моторов, дальше слегка корректируются акселем.

Если констатировать то гиры по осям считают углы очень точно, и уход измерений очень медленный.
Но только не совпадают эти углы с реальными углами относительно горизонта , при больших наклонах обоих
осей . Трудно представить ситуацию когда коптеру будут давать команду летать по кругу , меняя оси поочередно. ( передом, боком и задом )
Если мы летим прямо по камере то задаем только 1 угол. И при таком полете расчета по осям хватит на 100 %. Не совсем понятно зачем нам нужна ориентация , кроме использования компаса. Коптеры летают с наклоном 30-40 град.И если они наклонились больше, то в этом виновата стабилизация. А те кто летает в АКРО то как я
понимаю там все на гирах и полностью ручное управление.
Кстати аксель калибровать не обязательно. Я у себя сделал калибровку мозгов, в отличии от стандартной схемы где калибруют пульт. Стиками при висении выравниваю коптер и нажимаю кнопку.
Значения стиков записываются в ОЗУ и потом вычитаются из значений новых( есть задержка на запись) При включении коптера значения считываются из памяти .

trim_x =constrain ( 125 - EEPROM.read(0) , -6 , 6 ) ;
trim_y =constrain ( 125 - EEPROM.read(1) , -6 , 6 ) ;
trim_z =constrain ( 125 - EEPROM.read(2) , -2 , 2 ) ;

if (kom5==1 && kof==0)
{ kof=1;
EEPROM.write(0,BIT3); trim_x =constrain ( 125 - BIT3, -6 , 6 );
EEPROM.write(1,BIT4); trim_y =constrain ( 125 - BIT4, -6 , 6 );
EEPROM.write(2,BIT5); trim_z =constrain ( 125 - BIT5, -2 , 2 );
}

oshibka_x = ug_xg - dx - trim_x ;
oshibka_y = ug_yg - dy - trim_y ;

dx и dy углы с пульта.

Чёт я наверно не с той планеты?
наклонили мы ось x на 30 гр. повернулись вокруг неё ещё на 30 гр. что покажут ДУС и аксель, и как это смешать?
существует вообще 5 алгоритмов нахождения положения в пространстве, но только 2 из них удобоваримые (матричный метод и кватернионы) и заметьте во всех алгоритмах общий вектор по всем осям, а не выискивание отдельных векторов и потом коррекция их не знамо по чём…

Не совсем понятно зачем нам нужна ориентация , кроме использования компаса.

Насколько я понимаю для таких примочек как инс, компенсация центрифугальных ускорений да и собственно вообще стабилизация по GPS. А так да, если не вращаться по яву то такой подход выглядит рабочим для ручного управления.

что покажут ДУС и аксель, и как это смешать?

Проделал опыт : Наклонил по X на 30гр а потом на 30 по Y , смотрел по гирам. Гиры показывают 30 30 а аксель 30 36. Появилась ошибка 6 градусов по Y. ( гиры не стабилизированны по акселю)
Если мы летим прямо , по Х 30 гр и даем например в право стиком 30гр по Y,
то гиры стабилизируют наклон по Y до 30 гр по своему. Коптер летит наискось.
Через время коррекции гир они начинают стремится к 36 гр. Но с пульта идет
30 по X и 30 по Y . Гиры как бы перекрутили немного и уменьшат наклон.
Коптер некоторое время полетит не под 45 гр от прямого полета ,а чуть больше.
Если перекладка курсов будет не быстрая , то все должно быть отлично

Чтоб летать только по гирам то с пульта надо слать не абсолютные угы наклона,
а ошибку по осям.( плюс убрать коррекцию гир ) Чем больше отклонение стика от нулевой оси , тем больше ошибку должны слать. Будет постоянное вращение по оси , пока не вернём стик в 0.

Чтоб летать только по гирам то с пульта надо слать не абсолютные угы наклона, а ошибку по осям.( плюс убрать коррекцию гир ) Чем больше отклонение стика от нулевой оси , тем больше ошибку должны слать. Будет постоянное вращение по оси , пока не вернём стик в 0.

да и угол считать не надо, достаточно пропорции с угловых скоростей в управление мешать - это велосипед КУК
а если ещё аксель добавить со своими пропорциями, то это КК2 (второй КУК)