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

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

Samer:

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

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

oleg70:

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

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

Спасает то что летаем при небольшом угле , до 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 градус

Samer:

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

Samer:

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

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

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

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

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

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

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

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

rual:

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

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

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

alexeykozin:

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

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

каждый ПИД должен иметь отдельно ограничения ПД и И-составляющих.
Чтоб избежать переполнения по общему газу, его тоже надо ограничить до 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 из них удобоваримые (матричный метод и кватернионы) и заметьте во всех алгоритмах общий вектор по всем осям, а не выискивание отдельных векторов и потом коррекция их не знамо по чём…

Samer:

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

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

SergDoc:

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

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

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

Samer:

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

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

SergDoc:

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

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

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

alexeykozin:

обсуждаем как деградировать до кука

эээ не скажи, там в KK2 довольно интересный регулятор (ПИД в смысле)…

Samer:

Проделал опыт : Наклонил по X на 30гр а потом на 30 по Y , смотрел по гирам.

Сергей, мне тоже стало интересно: а почему не сделать “как положено” для нормальной ИНС ? В этом какой то глубинный замысел ? что б тратить на это время… в чем прикол то ?
(если уж идти по пути упрощения математики то можно вообще две гиры китайских на раму поставить будет летать “на ручках” тоже неплохо…)

oleg70:

Сергей, мне тоже стало интересно: а почему не сделать “как положено” для нормальной ИНС ?

На что хватает понимания то и делаю, а просто брать кусок кода не зная что и как работает не хочется ( в смысле математики ).
В принципе у меня все работает нормально.
Тогда надо брать готовые прошивки и возится с настройками.
Тема называется :
Создание СОБСТВЕННОЙ системы стабилизации.
Плохая или хорошая , но СОБСТВЕННАЯ .

Samer:

На что хватает понимания то и делаю, а просто брать кусок кода не зная что и как работает не хочется ( в смысле математики ). В принципе у меня все работает нормально. Тогда надо брать готовые прошивки и возится с настройками. Тема называется : Создание СОБСТВЕННОЙ системы стабилизации. Плохая или хорошая , но СОБСТВЕННАЯ .

Хорошая позиция, но только тогда непонятно упорство с которым вы отрицаете то решение, что вам предлагают. Вы же сами задали вопрос, почему так происходит - все отлично при движении раздельно по осям, но возникает ошибка при сложном движении. Вам говорят как это решается - и это единственный способ. И не настолько уж и сложный, алгоритм DCM к примеру достаточно прост. Но вы пытаетесь решить проблему вводом каких то посторонних сущностей и поправочных коэффициентов без какого-либо обоснования, повторяя то, что прошли самодельшики 10 лет назад и резльтатом которого стали рабочие автопилоты типа Мультивия или Ардупилота.

oleg70:

Сергей, мне тоже стало интересно: а почему не сделать “как положено” для нормальной ИНС ? В этом какой то глубинный замысел ? что б тратить на это время… в чем прикол то ?

Я хоть и не Сергей, своё мнение выскажу: качество применяемых нами датчиков не позволяет сделать 100% ИНС, с удержанием позиции без опоры хотя бы десятки минут. В основном борьба идет за плавность и точность перемещения с опорой на ГНСС, при достаточно сложных алгоритмах и местами весьма неустойчивых. КУКовские же алго весьма просты и устойчивы, некоторая проблема возникает если хотим прикрутить навигацию, но и это решаемо. Когда читал ветку про немецкий полетнег (МК вроде), то сложилось впечатление что алго там именно такой, и у мелкой китайшины тоже.
Ну и самое главное:

Samer:

Тема называется :
Создание СОБСТВЕННОЙ системы стабилизации.
Плохая или хорошая , но СОБСТВЕННАЯ .

К тому же если пока (или вообще ) нет желания заниматься навигацией, то нафига козе баян? Сначала закручивать вектора в оператор ориентации ( а некоторые это делают ещё и синусами-косинусами), а потом обратно раскручивать в вектора моментов стабилизации, ради чего, когда их можно взять сразу с датчиков?

В начале зимы было у меня желание написать статейку-конструктор про примитивную стабилизацию с раскладкой на пальцах, но увидев, что большинство на форуме обсуждает цветовую гамму нового фантика - забил… ну ещё и лень сильно поддакивала 😁

rual:

но увидев, что большинство на форуме обсуждает цветовую гамму нового фантика - забил…

rual:

К тому же если пока (или вообще ) нет желания заниматься навигацией, то нафига козе баян? Сначала закручивать вектора в оператор ориентации ( а некоторые это делают ещё и синусами-косинусами), а потом обратно раскручивать в вектора моментов стабилизации, ради чего, когда их можно взять сразу с датчиков?

Хм, может я чего то не понимаю, но при чем тут ИНС и удержание позици? Да самая элементарная вещь - коррекция по акселю - требует знания ориентации в виде хотя бы вектора. Как можно скорректировать две непонятные величины, которые мы получим после интегрирования гироскопа раздельно по осям, и связать их с ВЕКТОРОМ гравитации? В этом нет никакого математического смысла, кроме идеалных одномерных случаев.

alexmos:

коррекция по акселю - требует знания ориентации в виде хотя бы вектора

Ориентацию дает нормированный вектор акселя - вертикаль (0,0,1). Отклонение от вертикали создает проекцию оси гравитации на горизонтальные оси акселя, вот они в качестве пропорциональной и интегральной составляющей вводятся в ПИД, составляющая Д берется с оси ДУСа. Для мелких и шустрых может понадобится составляющая Д’ (угловое ускорение) - численное дифференцирование оси ДУС.

alexmos:

Как можно скорректировать две непонятные величины, которые мы получим после интегрирования гироскопа раздельно по осям, и связать их с ВЕКТОРОМ гравитации?

Почему непонятные? Вполне понятные, если ДУС не интегрировать. ДУС в данном случае выполняет роль демпфера угловой скорости, т.е. стремится удержать установленное положение с нулевой угловой скоростью, не важно как расположенное к вектору гравитации. Дополняя его проекциями вектора гравитации на горизонтальные оси акселя получаем двумерный синус отклонения от вертикали, который даст ПИ-составляющие, которые будут возвращать коптер в горизонт. Если сюда добавить дестабилизацию от РУ, то получим управление в горизонте(в АПМ вроде ЛИВЕЛ называется), если убрать аксель - получим управление угловыми скоростями (АКРО). Есть мат.модель, будет время соберу ардумодельку (хотел про это статью написать, но придется размазывать).
Ну и демпфер газа - смесь РУ_газа с вертикальной ось акселя (совпадающей с осью тяги), позволяет сделать более плавное руление по газу. Китайцы, которые этого не знают, будут в восторге.
Формулы я выше писал, они рабочие.