MultiWii - обсуждаем и отлаживаем Alt Hold
в оптимизированной версии результат получатся меньше на alt/ACC_BARO_CMPF
если так, то получается высота просто стремится к 0 всегда, и такой дампиг не оправдан ничем. CMPF её скорректирует конечно но зачем?
сам тестил + помоШника нашел (хороший пилот)… не полетело в итоге… раскачки, осцилляции, ломанные винты…
Ясно, спасибо за иныу. Интересно самому попробовать, т.к. если это действительно необходимо для углов, то тогда и высоте может помочь.
В стабилизации высоты все же другие и цели, и исходные данные. Там очень точные показания гиро, высокая скорость “исполнительнго механимза”, и задача убрать мелкие раскачивания. А в высоте - постоянный низкочастотный шум ±1м и слабая коррекция на движки, т…е все на-амного медленнее.
ну я говорил не про замену CF на медиан, а про предварительную фильтрацию данных с барометра медианом или для начала обычный НЧ попробовать поставить…
Обычный НЧ уже есть посредтсвом CMPF. Медианный - думаю ничего не даст кроме усложения вычислений. Мы же ставим дотстаточно высокий k фильтра CMPF и фактически задавливаем все шумы барометра…
Вот что подкинул ziss_dm: www.icee-con.org/papers/2006/pdf/EC2-08.pdf Как я понял, отличия от моего алгоритма в LPF-фильтре 3-его порядка, т.е. повышении крутизны спада АЧХ (если говорить в терминах электроники). Но там слишком много математики, может позже разберусь. И у меня PID-регулятор, а что там не понял.
если так, то получается высота просто стремится к 0 всегда, и такой дампиг не оправдан ничем
тогда нужно добавить alt/ACC_BARO_CMPF, т.е. alt-=err; поменять на
alt-= err - alt/ACC_BARO_CMPF;
если это действительно необходимо для углов, то тогда и высоте может помочь. В стабилизации высоты все же другие и цели, и исходные данные. Там очень точные показания гиро, высокая скорость “исполнительнго механимза”, и задача убрать мелкие раскачивания. А в высоте - постоянный низкочастотный шум ±1м и слабая коррекция на движки, т…е все на-амного медленнее.
нужно пробовать вобщем… практика покажет…
хотя в реализации алт-холд от ziss_dm ето уже проверено и успешно работает по отзывам многих, правда там ускорение из скорости высчитывается, а не берется из оси Z акселя…
Обычный НЧ уже есть посредтсвом CMPF. Медианный - думаю ничего не даст кроме усложения вычислений. Мы же ставим дотстаточно высокий k фильтра CMPF и фактически задавливаем все шумы барометра.
пробовал вчера добавить НЧ фильтр с высоким фактором (0.3-0.5гц) на сырые данные баро… палка о двух концах… шума стало заметно меньше в альт, но скорость гораздо медленее восстанавливается, а это совсем не плюс для стабилизации в пид регуле…
Вот что подкинул ziss_dm
осторожно! а то можно и мосК сломать! 😃
ziss таким образом устраняет конкурентов 😃
p.s. кстать errI в вычислении accZ плавно но верно корректирует дрейф… спецом пробовал вносить ошибку некорректной калибровкой акселя…
работает, но не могу до конца понять каким образом… интуитивно вроде как эффект обратной связи получется? можешь нарисовать на палцах? или тут если формулами не расписать то не понять? ну или как ты это понял?
Удалось полетать сегодня, в целом резултаты нормальные, но есть куда двигаться. Видео надо подрезать и выложу позже. И сегодня же вживую пощупал Parrot ADrone -да, это культурнй шок. ТАКОЙ стабилизации по высоте мне вряд ли добиться 😃
p.s. кстать errI в вычислении accZ плавно но верно корректирует дрейф… спецом пробовал вносить ошибку некорректной калибровкой акселя…
Ну собственно в этом и смысл алгоритма - найти “нулевую точку” акселя при помощи барометра, и далше показаня брать с акселя. Я уже писал, что таким образом решаются все проблемы с вибрационным дрейфом, температурным и т.д. акселя, и его показания можно уже рассматривать всерьез. В статье я как раз и объяснял как это решается, подробнее уже вряд ли объясню 😃
Ещё сделал мелкую доработку: коррекция газа при наклоне аппарата THROTTLE_ANGLE_CORRECTION, чтобы вертикальная составляющая осталась неизменной. Это должно помочь не терять высоту при начале движения. Работает только в stable mode. Возможно придется умножить на понижающий коэффициент, т.к. не уверен в линейной зависимости газа и косинуса угла.
Попробовал и это включить, не сработало. COS угла не отражает реланый спад подъемной силы при начале движения, он намного больше. Видимо, происходит сход со струи или какие то более сложные процессы, чем простой наклон вектора тяги. Если кто сталкивался с теоретическими или практическими исследованями в этой области, подскажите. Можно конечно подобрать коэффициент эксперементально, но не с моими возможностями по полетам 😃
Такой код дает довольно быструю реакцию на ускорение, по баро сглаживает. Акселерометр дает 2 всплеска противоположного знака при разгоне и торможении. В нештатной ситуации скорость acc_velocity, рассчитанная по изменению итоговой высоты, начинает принимать экстремальные значения, система идет вразнос.
//суммируем скорости на промежутках времени cycleTime для поиска средней скорости
acc.readAccel(&ax,&ay,&az);
acc_velocity += 9.80665f * cycleTime * (az - calib)/LSB;
acc_i ++;
//неспешно (зачем лишний шум) опрашиваем баро, суммируем для нахождения среднего: симметричные значения уничтожаются.
baro_loop +=cycleTime;
if (baro_loop > 0.05) //sec
{
dps.getAltitude(&baro_alt);
baro_alt_sum += baro_alt;
baro_i++;
baro_time =0;
}
//Alt_loop медленный цикл, позволяет усреднить скорость по акселю и найти среднюю высоту по барометру (успеваем накопить данные)
alt_loop += cycleTime;
if (alt_loop > 0.5) //sec
{
Altitude = 0.15f * Altitude + 0.85f * baro_alt_sum/baro_i + alt_loop * acc_velocity / acc_i;
Serial.println(Altitude, DEC);
baro_alt_sum = 0;
baro_i = 0;
acc_i =0;
alt_loop =0;
//начальная скорость для нахождения средней по акселю - рассчитывается по изменению высоты (с учетом баро и акселя)
acc_velocity = (Altitude - Altitude_prev) / alt_loop;
Altitude_prev = Altitude;
}
Господа, извините что вмешиваюсь, от понимания кода совершенно далек, просто возникла мысль из ряда “а что будет если?”.
Так вот, что если взять два bmp085,или любые другие, и из данных получаемых одновременно выделить “среднее арифметическое”,?
Возможно , как минимум проблему с"мусором" от части получится сгладить.
пощупал Parrot ADrone -да, это культурнй шок. ТАКОЙ стабилизации по высоте мне вряд ли добиться
както читал про него… вроде там open source… или только SDK к нему в открытых кодах… а если и сорсы открыты, то можно почерпнуть…
Ну собственно в этом и смысл алгоритма - найти “нулевую точку” акселя при помощи барометра, и далше показаня брать с акселя. Я уже писал, что таким образом решаются все проблемы с вибрационным дрейфом, температурным и т.д. акселя, и его показания можно уже рассматривать всерьез. В статье я как раз и объяснял как это решается, подробнее уже вряд ли объясню
статью конечно же читал… и не просто читал, а внимательно вникал…
интуитивно стало понятно, но математически на пальцах не получается понять каким образом именно интеграл ошибки становится корректирующей величиной проекции вектора ускорения…
COS угла не отражает реланый спад подъемной силы при начале движения, он намного больше.
я это понимаю так: тут уже скорее всего аэродинамика вход идет… при движении с горизонтальной скоростью, каждый из винтов типа “крыло” со своей подъемной силой, а в начале движения этой подъемной силы нет соот-но… теперь остается это расчитать 😉
что если взять два bmp085,или любые другие, и из данных получаемых одновременно выделить “среднее арифметическое”,? Возможно , как минимум проблему с"мусором" от части получится сгладить.
ради эксперимента можно попробовать, но как по мне так уже лучше сразу взять MS5611-01, тогда и в сонаре надобность отпадет
www.ebay.com/itm/…/260891831092
да и цена 30$… bmp085 брал за 20…
как я уже писал, мысль-идея-решение витает в воздухе в параллели… www.multiwii.com/forum/viewtopic.php?f=7&t=363&p=8…
Удалось полетать сегодня, в целом резултаты нормальные, но есть куда двигаться. Видео надо подрезать и выложу позже. И сегодня же вживую пощупал Parrot ADrone -да, это культурнй шок. ТАКОЙ стабилизации по высоте мне вряд ли добиться 😃
Попробовал и это включить, не сработало. COS угла не отражает реланый спад подъемной силы при начале движения, он намного больше. Видимо, происходит сход со струи или какие то более сложные процессы, чем простой наклон вектора тяги. Если кто сталкивался с теоретическими или практическими исследованями в этой области, подскажите. Можно конечно подобрать коэффициент эксперементально, но не с моими возможностями по полетам 😃
Я решил эту проблему при помощи сонара. Добавил в газ производную от высоты по сонару (дифф. составляющая) и проваливаться перестал. В Parrot ADrone стоит сонар, так что не удивительно. Сейчас занимаюсь такой же проблемой, но на другой платформе rcopen.com/forum/f123/topic263186 . Решение этой проблемы существенно должно упростится при наличии хорошего барометра. Я взял MS5611 , читаю с него данные с частотой 100 Гц . Далее буду усреднять комплиментарным фильтром с добавкой акселерометра. Скорость рассчитанную из данных акселерометра отфильтруем ФВЧ и это должно решить проблему накопления и ошибки по скорости. До конца не доходит идея с векторным произведением (статья с Хабра) . Мне казалось , что если разделить Az акселерометра на Cos (крена) и Cos(тангажа) то получим нужную составляющую? Или я не прав?
Скорость рассчитанную из данных акселерометра отфильтруем ФВЧ и это должно решить проблему накопления и ошибки по скорости.
После расчета высоты (с учетом барометра) скорость “acc_velocity” корректирую :
acc_velocity = (Altitude - Altitude_prev) / alt_loop;
После расчета высоты (с учетом барометра) скорость “acc_velocity” корректирую :
acc_velocity = (Altitude - Altitude_prev) / alt_loop;
А я просто умножал ее на 0.995 . Такой себе ФВЧ с частотой среза 1 Гц. Длительность цикла 5 миллисекунд.
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 и обзывать осмысленными именами и подписывать, чтобы проще было понять потом логику и физический смысл алгоритма.