MultiWii - обсуждаем и отлаживаем Alt Hold
Accel_2 = Accel_2 + (float)(accel_Z / DCM_Matrix[2][2]-Accel_2)/500.0;
delta_A = (float)(accel_Z / DCM_Matrix[2][2] -Accel_2)*0.00204035;
Speed_A = Speed_A*0.99 + delta_A * G_Dt;
H_A = H_A + Speed_A*G_Dt + (Baro_alt - Baro_graund - H_A)* 0.1 ;
Вот такой код где DCM_Matrix[2][2] = Cos (крена) х Cos(тангажа)
Все на столе. Поднимаю на уровень монитора ноутбука На втором подъеме качаем по крену и тангажу градусов на 30 . Потом несколько раз поднимаем. Высота в метрах. 0.1 сек на деление
Усреднили ускорение по 10 измерениям и баро по 50
Accel_2 = Accel_2 + (float)(accel_Z / DCM_Matrix[2][2]-Accel_2)/500.0;
delta_A = (float)(accel_Z / DCM_Matrix[2][2] -Accel_2)*0.00204035;
delta_A_midd = delta_A_midd + (delta_A - delta_A_midd)*0.1;
Speed_A = Speed_A*0.99 + delta_A_midd * G_Dt;
H_A = H_A + Speed_A*G_Dt + (Baro_alt - Baro_graund - H_A)* 0.02 ;
Измерения в течении одной минуты.Высота по баро уплыла на20 см. На плоской вершине качали с углами больше 60 град.
После расчета высоты (с учетом барометра) скорость “acc_velocity” корректирую :
acc_velocity = (Altitude - Altitude_prev) / alt_loop;
Я тоже так пробовал. Но при низком времени цикла 3мс,шумит сильно…т.е. шумит высота а разность на малом dt тем более.
на пальцах не получается понять каким образом именно интеграл ошибки становится корректирующей величиной проекции вектора ускорения…
Потому что ошибка от барометра идет постоянно, и ускорение корректируем постояно, но только I-составляющая содержит постоянную часть ошибки, а P и D плавают в ±.
ради эксперимента можно попробовать, но как по мне так уже лучше сразу взять MS5611-01, тогда и в сонаре надобность отпадет
Да, точность на видео впечатляет. С таким барометром сонар и правда не нужен.
Я решил эту проблему при помощи сонара. Добавил в газ производную от высоты по сонару (дифф. составляющая) и проваливаться перестал
Это-то понятно - мой алгоритм тоже стабилизирует высоту после начала движения, но если бы мы заранее знали, что вот сейчас будет нужна коррекция и её точная величина - то время стабилизации и отклонения уменьшится существенно.
Я взял MS5611 , читаю с него данные с частотой 100 Гц
Ждем результатов и кода.
До конца не доходит идея с векторным произведением (статья с Хабра) . Мне казалось , что если разделить Az акселерометра на Cos (крена) и Cos(тангажа) то получим нужную составляющую? Или я не прав?
Векторное произведение, вернее проекция - это самый простой и 100% точный способ который можно применить на основе уже вычисленных ране данных. Косинус не подходит - AccZ мало, нужны все компоненты вектора (AccZ не учитывает горизонтальные ускорения при угле больше 0)
acc_velocity = (Altitude - Altitude_prev) / alt_loop;
Если для рассчета высоты ACC все равно надо считать скорость в интеграторе, зачем её считать “обратным способом” исходя из неточных показания барометра? Гдавный таргет алгоритма - использовать акселерометр по максимуму, а барометр только для коррекции ошибки. Хотя с более точным барометром можно и пересмотреть стратегию.
Видео тестирования. Во время съемок ручку газа не трогал ни разу (кроме посадок и выхода на режим Alt Hold)
www.youtube.com/watch?v=O8elQs1MZzQ
Результаты:
- надо увеличивать D (на нужную высоту выходит толко после 2-й осцилляции), стояло на пределе в 50, так что надо в коде увеличивать раза в 2-3 коэффициент
- барометр держит ± 1 метр, что соотвествует его возможностям.Больше вряд ли из него можно что то выжать
- Реакция на резкие возмущения хорошая, на наклоны и горизонтальные ускорения - реакции нет вообще, так что тут зачет.
- При начале движения просаживается вниз, потом его алгоритм компнсирует но при остановке идет перекомпенсация и подпрыгивает вверх. Тут тоже все в прееделах рассчетного. Выход - или делать более жексткий и быстрый PID-регулятор, либо сразу внести корректировку угла (о чем писал выше)
-Самое главное - он СТАБИЛЕН. Т.е. он ни разу за время облета 4-х паков не приложился об землю и не улетел выше 4-х метров.
Это-то понятно - мой алгоритм тоже стабилизирует высоту после начала движения, но если бы мы заранее знали, что вот сейчас будет нужна коррекция и её точная величина - то время стабилизации и отклонения уменьшится существенно.
Векторное произведение, вернее проекция - это самый простой и 100% точный способ который можно применить на основе уже вычисленных ране данных. Косинус не подходит - AccZ мало, нужны все компоненты вектора (AccZ не учитывает горизонтальные ускорения при угле больше 0)
А откуда заранее знать что коптер захочет провалиться. Он просто компенсирует провал.
Вопрос такой - так векторное произведение или проекция. Векторное произведение - это вектор направленный перпендикулярно плоскости, в которой лежат сомножители и по величине равен произведению модулей на синус угла.
А коптер летает хорошо. Только по сонару Пиды побольше поставить. Лучше стабилизироваться будет. По моему опыту в 2 раза больше чем по баро.
А откуда заранее знать что коптер захочет провалиться. Он просто компенсирует провал.
Ну он же всегда проваливается при наклоне и начале движения, и всегда подпрыгивает при остановке. Есть какая-то закономерность, обусловленная физикой.
Вопрос такой - так векторное произведение или проекция
Нет, конечно же не векторное, а скалярное, описка 😃. Нам нужна проекция вектора ускорения на вектор ориентации (по которому стабилизируем углы, в MultiWii он всегда направлен вниз). Из определения ru.wikipedia.org/wiki/Скалярное_произведение обращаем внимание на строку “Данной операции соответствует умножение длины данного вектора x на проекцию другого вектора y на данный вектор x” ну и дальше все просто.
А коптер летает хорошо. Только по сонару Пиды побольше поставить. Лучше стабилизироваться будет. По моему опыту в 2 раза больше чем по баро.
Спорный вопрос - во первых, два набора пидов усложнят настройку. А во вторых, сонар не всегда точен, и в некоторых условиях может очень сильно лажать… Вот почему его также надо корректировать ускорением:
Ну он же всегда проваливается при наклоне и начале движения, и всегда подпрыгивает при остановке. Есть какая-то закономерность, обусловленная физикой.
Спорный вопрос - во первых, два набора пидов усложнят настройку. А во вторых, сонар не всегда точен, и в некоторых условиях может очень сильно лажать… Вот почему его также надо корректировать ускорением:
Я это не сам придумал.
Коррекция газа от угла наклона введена в Арду 2.0 давно.
Два набора Пидов в Арду были более года еще в ArducopterNG. Можно летать и на одном - но тогда с сонаром больше плавать будет по высоте. Это проверено. Если на настройках сонара летать по баро излишне дергается.
Корректировать сонар по акселерометру - простым алгоритмом не обойдешся. Там проще отбрасывать некорректные результаты измерений.
Другое дело в баро - там акселем компенсируется шум баро-то есть шум убирается фильтрацией, а возникающее при этом запаздывание , мешающее работе Пид регулятора и приводящее к незатухающим колебаниям компенсируется составляющей рассчитанной по акселерометру.
Но намного легче будет с хорошим баро. При высокой частоте измерений и меньшем уровне шума его легче фильтровать, а медленный дрейф баро можно компенсировать измерениями по сонару - при желании, но алгоритм оценки достоверности и пригодности для коррекции тоже вопрос не тривиальный. Если на поле, то просто, а если на горбатой местности-? 😃
как я уже писал, мысль-идея-решение витает в воздухе в параллели… www.multiwii.com/forum/viewto...3&p=8871#p8840
- еще один рабочий алгоритм www.multiwii.com/forum/viewtopic.php?f=7&t=363&sta…
BMP085
MS5611
-Самое главное - он СТАБИЛЕН. Т.е. он ни разу за время облета 4-х паков не приложился об землю и не улетел выше 4-х метров.
Результат очень хороший. Я приятно удивлен. Я не верил, что из 085 можно такое выжать.
Мне в прошлом году времени для дотошного изучения баро не хватило (были более интересные проблемы). В этом году вернулся к баро, но уже с MS5611. Судя по результатам на столе с MS5611 получится. Но я поражен Вашими результатами с 085. Я думаю , что китайцев с NAZA вы уже догнали.
Был бы благлдарен,если бы Вы расписали математику, но без привлечения векторов. Не в обиду и скалярного произведения нет там.
Свои результаты я выше привел. Код полностью в соседней теме "High end Low cost Quatrocopter"Саму стабилизацию по высоте чуть позже добавлю в программу.До этого при стабилизации по высоте я использовал газ висения, но на этой платформе нужен алгоритм как его измерять. В Арду просто, там есть лог и полетавши берутся данные из лога. Использовать значение газа в момент переключения не очень здорово. Каждый раз аппарат точно вывешивать хлопотно.
А дальше появляется любопытная возможность повозится и с горизонтальной составляющей акселерометра и компенсировать воздействия от порывов ветра и прочее.
Результат очень хороший. Я приятно удивлен. Я не верил, что из 085 можно такое выжать
Спасибо. Уже есть пара идей насчет улучшения. Сейчас явно не хватает силы PID-регулятору высоты, но если его усиливать, все скачки барометра вылезут ещё больше. Так что надо ещё сильнее сглаживать вычисленную высоту. И для сонара надо что то придумывать, если увеличивать коэффициенты PID раза в три, будет сильно подкидывать на кочках, а это не есть гуд. Сонар должен отслеживать медленно меняющийся рельеф местности, но мелкие кочки пропускать.
Был бы благлдарен,если бы Вы расписали математику, но без привлечения векторов. Не в обиду и скалярного произведения нет там
Без векторов я не смогу объяснить 😦 В коде “в лоб” взято определение скалярного произведения и длины вектора (из википедии)
(accADC[0]*EstG.V.X + accADC[1]*EstG.V.Y + accADC[2]*EstG.V.Z) * InvSqrt(fsq(EstG.V.X) + fsq(EstG.V.Y) + fsq(EstG.V.Z),
что соответсвует A_B_projection = (Ax*Bx + Ay*By + Az*Bz)/sqrt(Bx*Bx + By*By + Bz*Bz)
Каждый раз аппарат точно вывешивать хлопотно.
Точное вывешивание не нужно, можно при любом газе включить удержание. Разница будет только во времени стабилизации PID-регулятором.
При полете по программе и необходимости выйти на этот режим автоматически, нужно все же знать примерный газ удержания. Или делать автовыод на режим, медленно увеличивая газ и следя за вертикальной скоростью (установка вблизи нуля). У меня в общем пока такой задчи не стоит, не думал особо.
А дальше появляется любопытная возможность повозится и с горизонтальной составляющей акселерометра и компенсировать воздействия от порывов ветра и прочее.
Я уже пробовал, результаты тут: www.multiwii.com/forum/viewtopic.php?f=7&t=685
Тогда у меня было чуть меньше опыта, но со своим выводом в конце темы согласен и теперь.
еще один рабочий алгоритм www.multiwii.com/forum/viewto...start=90#p8876
Занятно, человек вообще отказался от акселерометра 😃 Сразу предположу, что он не сможет выставить более-менее рабочий PID, т.к. D-часть брать неоткуда. Судя по видео, у него стоит очень слабый PID, и вряд ли он удержит коптер реалных ситуациях.
Сейчас явно не хватает силы PID-регулятору высоты, но если его усиливать, все скачки барометра вылезут ещё больше. Так что надо ещё сильнее сглаживать вычисленную высоту. И для сонара надо что то придумывать, если увеличивать коэффициенты PID раза в три, будет сильно подкидывать на кочках, а это не есть гуд. Сонар должен отслеживать медленно меняющийся рельеф местности, но мелкие кочки пропускать.
Опять таки все просто. Ставится ограничение на Пид регулятор (у меня +(100-150) -(40-60) мкс. И высоту держит точно и не подпрыгивает особо. Но газ при этом точно выставлять нужно.
По І составляющей ±30 мкс.
Предлагаете ограничивать управляющее воздействие (то есть коррекцию газа)? Не выход, мне кажется… Иначе регулятор будет работать в нерасчетном режиме. И регулятору нужна свобода действий, особено сильному и быстрому 😃
К посту rcopen.com/forum/f123/topic265409/28 - не могли бы вы побьольше комментариев расставлять? Самое главное в нашем общении тут не код, а идеи. И константы лучше выносить в #define и обзывать осмысленными именами и подписывать, чтобы проще было понять потом логику и физический смысл алгоритма.
Предлагаете ограничивать управляющее воздействие (то есть коррекцию газа)? Не выход, мне кажется… Иначе регулятор будет работать в нерасчетном режиме. И регулятору нужна свобода действий, особено сильному и быстрому 😃
К посту rcopen.com/forum/f123/topic265409/28 - не могли бы вы побьольше комментариев расставлять? Самое главное в нашем общении тут не код, а идеи. И константы лучше выносить в #define и обзывать осмысленными именами и подписывать, чтобы проще было понять потом логику и физический смысл алгоритма.
Просто указываю диапазон изменения длительности управляющего импульса на регулятор в микросекундах. У нас разные программы и я пытаюсь свести к сравнимым величинам.
В части свободы регулятора . Без ограничения у меня не получалось получить жесткое удержание без болтанки и быстрое достижение заданной высоты.
И вопрос - EstG.V.Z -это проекция G на ось Z.
Уже есть пара идей насчет улучшения. Сейчас явно не хватает силы PID-регулятору высоты
высадил кучу паков на выходных… при П более 2-х начинает колбасить вверх вниз, т.е. можно смело уменшать делитель в ПИД регуле…
Hекоторые идеи по улучшению:
- пробовал добавлять ПД регуль по скорости… Точнее Д из ПИД по высоте переехал в П по скорости (что одно и тоже) + в качестве Д по скорости взял ускорение как описывал выше + поставил деадбенд на ПД +/20, но не просто обрубил значения менее 20-ти, а с вычитанием в 20, если ПД более 20-ти (т.е. смещение значений к ценру… типа как вишном ролл/питч деадбенде на аппу)… шум сразу пропал, что отлично и при довольно высоких коэф. чувствительность ПД регулятора не падает…
Далее занулял ПИ по высоте, а ПД по скорости брал довольно агрессивными. При рывке вверх и движении с вертикалной скоростью 1-2м/с практически мгновенно (~0.5сек) тормозит коптер при активировании альт-холд… но при движенни вниз результат немного хуже. Это скорее всего из-за явной нелинейности моего акселя по оси Z (+128/-152)…
За счет ускорения поднимается фронт скорости, а также получяется более крутой спад. В отрицателную часть уходит лишь немного в конце… так что можно смело исползовать ускорение, тем более что ускорение гораздо меньше шумит в сравнении со скоростью…
где зеленым-скорость, синим-ускорение, красным-сумма.
для демо: вниз рукой получалось протолкнуть макс. на 40-50см! Также думаю это даст огромный плюс при порывах ветра…
- т.к. “быстрые” составляющие (ускорение и скорость) высчитываются из акселя, думаю можно смело уменьшать частоту алт-холд интегратора/эстиматора и отталкиваться от частоты среза ФНЧ акселерометра. Теоритически конечно чем выше самплинг raw данных с акселя, тем точнее будут показания скорости после интегратора, но при этом придется поднять ФНЧ (к примеру с 20гц на 50 гц), чтобы не получать усредненные raw данные с акселя и не делать “пустых” вычислений. НО с поднятием частоты среза получим чвствительность к вибро. И результат будет только хуже как в алт холд, так и стаб/левел моде.
Золотая середина тут на мой взгляд - это 20-25гц ФНЧ акселя и 40-50гц частота интегратора соот-но… я как то убедил в этом АлексаВПариже (главный разработчик вии) и он для бма180 выставил 20гц, для бма020 25гц (уже был, т.к. это нижний предел) и для адхл345 тоже 25 гц, чтобы привести все аксели к частоте алт-холд интегратора…
www.multiwii.com/forum/viewtopic.php?f=8&t=849&sta…
www.multiwii.com/forum/viewtopic.php?f=8&t=849&sta…
тут тоже писал об этом:
rcopen.com/blogs/83206
также при таком подходе съэкономим ресурс процессора…
Так что надо ещё сильнее сглаживать вычисленную высоту
в догонку ко второму пункту, еще один плюсик… т.к. цикл вии ~300гц, а интегратор будет настроен на 40-50гц, то между итерациями интегратора можно собрать 6-7 raw самплов с баро и отдать их медианному фильтру… т.е. собираем 7 значений, сортируем, берем 4-е (середину). Шума определенно станет меньше…
Самое главное - он СТАБИЛЕН. Т.е. он ни разу за время облета 4-х паков не приложился об землю и не улетел выше 4-х метров.
а я много раз приложился пока ПИДы крутил… особенно когда сколол кончик пропеллера и пошли вибрации, стаб. мод еще кое как устоял (т.к. у меня в иму точный флоат фильтр на аксель с большим LPF фактором на 0.3-1гц), а вот аль-холд сразу направил коптер в планету 😃 но тут вполне известная проблема, было скорее всего просядание по оси Z от вибраций не смотря на выставленный диапазон 8g…
А коптер летает хорошо. Только по сонару Пиды побольше поставить. Лучше стабилизироваться будет. По моему опыту в 2 раза больше чем по баро.
общая теория такова: чем точнее сенсор, тем агрессивнее могут быть ПИДы… а сонары ведь разные бывают 😉
Спорный вопрос - во первых, два набора пидов усложнят настройку.
Можно летать и на одном - но тогда с сонаром больше плавать будет по высоте. Это проверено. Если на настройках сонара летать по баро излишне дергается.
когда устаканятся ПИДы для баро, можно подобрать коэф. для вычисления ПИДов сонара по ПИДам баро, либо проще - один коэф. для сонар from баро ПИД.
В части свободы регулятора . Без ограничения у меня не получалось получить жесткое удержание без болтанки и быстрое достижение заданной высоты.
высадил кучу паков на выходных… при П более 2-х начинает колбасить вверх вниз, т.е. можно смело уменшать делитель в ПИД регуле…
На оба вопроса ответ один - надо D увеличивать. (т.е. коэффициент при скорости изменения ошибки). Но тут как раз к нему высокие требования, если он сильно шумит и сам прыгает, то только хуже будет. Поэтому я сейчас думаю над тем как сделать максимально полезным акселерометр в связке с барометром.
У меня на видео D=50 и я бы поставил выше, но в ГУИ это предел. умножу на два и посмотрю что будет.
И вопрос - EstG.V.Z -это проекция G на ось Z.
Да, это Z- компонент вектора ориентации, т.е. настоящий G (а не тот что акселерометр меряет)
За счет ускорения поднимается фронт скорости, а также получяется более крутой спад. В отрицателную часть уходит лишь немного в конце… так что можно смело исползовать ускорение, тем более что ускорение гораздо меньше шумит в сравнении со скоростью… где зеленым-скорость, синим-ускорение, красным-сумма.
Ага, хороший пример. Добавлю Vel-D и себе, но надо будет провести читсый эксперимент с ним и без.
поставил деадбенд на ПД +/20
Тоже интересное решение, но оно отключает Vel-PD при медленном приближении к цели. Т.е. если ли смысл убирать совсем уж мелкий шум (если ты говоришь что коэффициенты высокие и чувствительность не падает)?
- т.к. “быстрые” составляющие (ускорение и скорость) высчитываются из акселя, думаю можно смело уменьшать частоту алт-холд интегратора/эстиматора и отталкиваться от частоты среза ФНЧ акселерометра
А в чем смысл делать дискретным вычисления? Только для экономи времени? Да, экономим, но также рвем время цикла (каждый 10-й длится дольше при работе алгоритма), и неизвестно что хуже.
И второе, я точно не знаю как работает датчик. Лучше сразу учеть все варианты его настройки (ну и разные типы датчиков, вот барометр получше уже на 100Hz запускается). Если в дачике настроен фильтр 25Hz, разве из этого следует, что в течение 40мс показания не меняются с него? Это как взять слегка размытую картинку (Гаусс ~ LPF) и разбить на большие пиксели - разница все равно будет заметна глазу.
А у нас интеграторы, там каждая мелочь важна.
а вот аль-холд сразу направил коптер в планету но тут вполне известная проблема, было скорее всего просядание по оси Z от вибраций не смотря на выставленный диапазон 8g…
Это важный момент, попробуй разобраться. Если действительно быстро проседает ACC - весь алгортм летит к чертям. ACC должен быть надежным. Виброизолируй его что ли наконец 😃
когда устаканятся ПИДы для баро, можно подобрать коэф. для вычисления ПИДов сонара по ПИДам баро, либо проще - один коэф. для сонар from баро ПИД.
Да, так проще всего. Вынести множитель в конфиг и все.
Ещё я долго думал как отделаться от скачков высоты сонара при облете препятствий - ведь по акселю мы точно знаем, что высота так быстро прыгнуть не может, и можем точно вычислить высоту препятсвия (по моему так сделано в Parrot Ar.Drone - если ему перекрыть сонар, он не прыгает вверх, а стоит на месте).
Но вот потом любая попытка подстроиться под новую высоту приведет к непрятностям, четкого решения не вижу. Но есть пара идей которые надо попробовать.
предложение-пожелание althold мастерам, для fpv полетов не хватает ограничения высоты, т.к. по камере не всегда понятно как близко земля и можно сильно шлепнуться, поэтому хочется (но не можется) дополнительный мод ограничения высоты полета:
- где то в прошивке (а потом и в GUI) вставлять минимальную высоту (допустим 2 метра), до которой коптер может снижаться
- так как высота явно маленькая то тут будет достаточно одного сонара (хотя не факт)
- повесить режим на тумблер, чтоб сесть можно было =)
имхо можно даже автопосадку (да и взлет, как у AR.drone) сделать, медленно уменьшая пороговое значение снижения и газ
Я пока по FPV не летаю, но планирую 😃 Действительно было бы удобно. Я так понимаю, это ограничение не должно быть связано с AltHold.
В ГУИ лезть неохота. Можно для начала сделать в конфиге, но вот как его выключать, чтобы можно было сесть?
Крутилки у меня на 6-канальном пульте зкончились уже (одна на режимы другая на камеру).
Я так понимаю, это ограничение не должно быть связано с AltHold.
не должно - это как отдельный режим, сотвествено вкл/выкл как любой другой режим, хз где каналов найти под всё и сразу =)
Вот ещё момент: сонар штука такая что может внезапно “потерять сигнал”, и что тогда делать? слушаться газа (и тогда все зависит от скорости реакции пилота), или оставить газ в том положении что было до потери сигнала, и слушаться газа только “вверх”? Тогда войдем в цикл бесконечного полета в космос, если сигнал с радара так и не вернется. Думаю тут надо и барометр привлекать, на случай сбоев сонара, так как на низкой высоте любой просчет алгоритма будет критическим.
идея в том что когда потеря сигнала - код как раз и не работает (т.е. улетели метров на 20 - сонар явно потеряет сигнал), а вот начали снижаться и сонар почуял высоту - тормозим если меньше 2 метров
косяк вылезает: как узнать - потеря сигнала из-за неисправности (или сигнал рассеялся) или из-за высоты - вот видимо без баро никак тут
а с другой стороны - какова вероятность что сигнала долго нет из-за глюка/поверхности?
как вариант:
- считать высоту такой же, пока нет сигнала N циклов,
- если N>порог_ошибок, то считаем что улетели и забиваем на код ограничения высоты,
- если сигнал вылез, N=0 и go to 1
т.е. если летим над странной поверхностью низко, то высота всегда есть, хоть и примерная, если давно нет данных - значит высоко и так летим или плюхнулись давно =)
А в чем проблема ориентироваться на высоту Сонар+баро. Просто должен включатся AltHold, как только высота становится меньше 2м. Поднялись выше - отключается (включается предыдущий режим).