Создание собственной системы стабилизации
Просмотрев множество вариантов систем стабилизации полёта, www.kkmulticopter.com/index.p...tics&Itemid=65 - эту штуку собрал сам, немецкие бутерброды не по карману, есть мысли по созданию собственного УНИВЕРСАЛЬНОГО контроллера для многомоторных систем, как пообещал знакомый программист не без исключения зачатков искуственного интеллекта (надеюсь). Преследую цель бесперебойной работы при потере связи с управляющей системой, выполнение задания и возврат на точку старта. Повторяюсь - бюджет очень урезан, каждое устройство будет собиратся вручную. Если есть мнения прошу кидаться помидорами. Старт разработок Июль 2011г.
ИИ здесь вреден. Интеллект и так есть - это интеллект пилота. Ну ка, качество ручной стабилизации по крену при обычном полёте и когда будете находу выводить формулы вычисления углов отклонения элеронов? А отталкиваться надо от многоканального астатического регулятора. Разнообразные регуляторы и системы автоматического управления - моя профессия и на сколько они на самом деле просты могу рассказать, как специалист. Для универсальности делайте автопилот адаптивным.
Угол_гир_х= Угол_гир_х + данные_гир_х *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.
Осталось только смешать это всё в сумматоре, для конфигурации “+” будет выглядеть вот так:
мотор_перед = -шим_танг+ шим_газ;
мотор_задний = шим_танг+ шим_газ;мотор_левый = -шим_крен+ шим_газ;
мотор_правый = шим_крен+ шим_газ;
Летать будет как по рельсам.
Если интересно, могу дорисовать компас.
мотор_левый = -шим_крен+ шим_газ
некатит. нужно проверять нет ли лимитов по шиму вывоимому на мотор и если есть то в зависимости от приоритетов (удержание левела - удержание высоты - удержание курса) к примеру если сумма пвм на мотор больше максимума по калибровке то нужно вычесть из опозиционного мотора избыток
при калькуляции шима нужно учитывать компенсацию моментов вращения - иначе не избежать раскачки
некатит. нужно проверять нет ли лимитов по шиму вывоимому на мотор и если есть то в зависимости от приоритетов (удержание левела - удержание высоты - удержание курса) к примеру если сумма пвм на мотор больше максимума по калибровке то нужно вычесть из опозиционного мотора избыток
при калькуляции шима нужно учитывать компенсацию моментов вращения - иначе не избежать раскачки
Согласен со всем, но оправдаюсь, я нарисовал только схему, не загромождал дополнительными проверками и пороговыми значениями. А так правильно:
- каждый ПИД должен иметь отдельно ограничения ПД и И-составляющих.
- Чтоб избежать переполнения по общему газу, его тоже надо ограничить до 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.