MultiWii - обсуждаем и отлаживаем Alt Hold
Хорошо, не буду спорить - возможно, D в скорости действительно нужен. Любая теория может быть опровергнута практикой 😃
Я специально вынес accZ в глобалные переменные чтобы не считать его “задом наперед” в пид-регуляторе. Умножь на acсZ на kD и проведи тесты 😃
Кстати, я никогда не сморел как устроен ПИД-регулятор углов. Какого хрена там D часть считается от ускорения, может в level-mode его есть смысл считать как раз от скорости изменения угла??? Получается, мы исполуьзем угол, интеграл угла, и ускорение (притом видно, что оно не очень хорошо себя вело, т.к. используется двойное сглаживание) . Но полностью игнорируем угловую скорость, а ведь она не такая шумная. Надо попробовать, может станет ещё стабильней 😃
Нет, P - это ПРОПОРЦИОНАЛЬНАЯ составляющая, т.е. это и есть ошибка, без dT. P = err * kP, а D - это ДИФФЕРЕНЦИАЛЬНАЯ, D = derr/dt * kD
tak, nado polomat’ libo moe predstavlenie o klassik PID, libo tvoe 😉
esli abstragirovat’sya ot koeficientov regulyatora i rassmatrivat’ odnu iteraciyu, to D budet propocionalna dvoynomu differencialu vhodnih dannih, gde:
- perviy diff eto ERROR
- vtoroy diff eto (ERROR - prev_ERROR)
v etom legko ubedit’sya esli na vhod podat’ k primeru rasstoyanie (ili ugol), to v GUI uvidish’ uskorenie (libo uglovoye uskorenie), t.e. raznopolyarnie vspleski…
vot prostoy primer iz wikipedii:
previous_error = setpoint - process_feedback
integral = 0
start:
wait(dt)
error = setpoint - process_feedback
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
goto start
Любая теория может быть опровергнута практикой
vot eto tochno! inogda po grafikam vse nu tak krasivo! 😃
вот блин! короче на работе надо работать, а не в прерывании думу думать… пока ехал домой, все понял 😃
сорри за путаницу с дифференциалами… все верно Д это скорость, при исходной “высота”… и главное что примерно пол года назад я с этим довольно досконально разобрался 😃:
rcopen.com/forum/f123/topic221574/2985
rcopen.com/forum/f123/topic221574/2990
rcopen.com/forum/f123/topic221574/2998
rcopen.com/forum/f123/topic221574/3003
rcopen.com/forum/f123/topic221574/3005
Кстати, я никогда не сморел как устроен ПИД-регулятор углов. Какого хрена там D часть считается от ускорения, может в level-mode его есть смысл считать как раз от скорости изменения угла??? Получается, мы исполуьзем угол, интеграл угла, и ускорение (притом видно, что оно не очень хорошо себя вело, т.к. используется двойное сглаживание) . Но полностью игнорируем угловую скорость, а ведь она не такая шумная. Надо попробовать, может станет ещё стабильней
так там в PD части, комплексного PI-PD регулятора, скорость и используется для P, а ускорение для D части, которое сложновато понять, но именно оно задает скорость-плавность системы…
if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; //16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; //32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; //16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; //32 bits is needed for calculation
т.е. там используется и угол, и интеграл угла, и скорость, и ускорение…
Хорошо что разобрались. Я гляну еще код, может и правда недоглядел. А ускорение в альт холд попробую включить, чисто ради интереса. К субботе вроде у нас погода нормализуется, попробую полетать.
Александр, а когда в режиме Alt Hold из состояния висения начать двигаться вперед, куда уносит коптер - вверх или вниз?
а когда в режиме Alt Hold из состояния висения начать двигаться вперед, куда уносит коптер - вверх или вниз?
сорри… не помню 😃
кстать попробовал корректировать высоту по высоте из баро в комплиментарном фильтре, т.е. заменил это
// Integrator - altitude, cm
alt+= vel * cycleTime * VEL_SCALE;
// Apply ACC->BARO complementary filter
alt-= err;
на это
alt += vel * cycleTime/1000000.0f;
alt = (alt*ACC_BARO_CMPF + sensorAlt)/(ACC_BARO_CMPF+1);
визуально вроде как стало меньше шумов в alt и vel… в принципе логично т.к. ошибка (alt - sensorAlt) больше шумит, чем просто sensorAlt, т.к. в ошибке шум не только баро, но и акселя…
Также заметил что при движении с постоянной вертикальной скоростью, показания скорости в гуи угасают, т.к. видимо, пид пытается свести скорость к нулю. Однако в динамике при быстрых изменениях отрабатывает отлично… потому не критично наверное…
vel+= (accZ - err*ACC_BARO_P - vel*ACC_BARO_D) * cycleTime * accScale;
Смотрел в гуи, что дает ускорение в Д состовляющей (по гирам)… увидел значительное увеличение крутизны пика управляющего воздействия на выходе… т.е. в алт холд пид регуле ускорение по идее точно не будет лишним, тем более что если коптер покое, то оно сидит себе в нуле и не шумит как скорость…
по поводу улучшайзеров:
- было бы неплохо думаю поставить фильтр на сырые значения баро… читал что для баро сенсора медиан лучше всего…
- думаю есть смысл поставить велосити деадбенд на выходе из иму, т.к. велосити шумит ~ в диаппазоне +/-10…15
з.ы. на офф. форуме опять активировались с альт. холдом. Кто первый?! 😃
www.multiwii.com/forum/viewtopic.php?f=8&t=562&sta…
www.multiwii.com/forum/viewtopic.php?f=7&t=363&sta…
чел стенд смастерил
пишет что можно теперь алгоритмы в реалтайме через матлаб. тестить!
кстать попробовал корректировать высоту по высоте из баро в комплиментарном фильтре, т.е. заменил это
Это я применил небольшую оптимизацию, чтобы не дублировать “тяжелы” умноженя и деления. На самом деле твоя фомула и моя совершенно одинаковы, это в обоих случаях комплементарный фильтр. Я проверил, одна формула сводится к другой раскрытием скобок.
Также заметил что при движении с постоянной вертикальной скоростью, показания скорости в гуи угасают
Ага, так и должно быть - это работает D-составляющая, она “гасит” скорость. Поэтому я её взял очень небольшую, и осциляции она гасит медленно. Но совсем без нее нелзя, иначе уйдет в раскачку.
Смотрел в гуи, что дает ускорение в Д состовляющей (по гирам)… увидел значительное увеличение крутизны пика управляющего воздействия на выходе… т.е. в алт холд пид регуле ускорение по идее точно не будет лишним, тем более что если коптер покое, то оно сидит себе в нуле и не шумит как скорость…
Пока трудно представить ситуацию, чтобы ускорение “резко” рвануло при удержании высоты. А при плавных возмущенях, каковые в основном и наод держать - шумом в ускорении все же нельзя пренебрегать, он становится сравним с измеряемой величиной. Поэтому я и против привлечения ускорения в PID-регулятор - только лишние дергания для моторов.
В удержании угла то же самое - если ставить высокий D на ROLL и PITCH - начинается кобасня на выходах мотров. И хотя это незаметно и они по прежнему стабилизируют хорошо, все же мне кажется что текущее решене неправильное - не нужно учитывать ускорение в PID регуляторе. И по теории не нужно, и логика это подсказывает.
- было бы неплохо думаю поставить фильтр на сырые значения баро… читал что для баро сенсора медиан лучше всего…
Попробуй сделать. Вполне допускаю, что комплементарный фильтр не самый лучший для барометра. У меня времени нет сейчас, и не охота пока усложнять алгоритм - уже и так слишком много теории, хочется попрбовать сначала, что текущее решение дает.
думаю есть смысл поставить велосити деадбенд на выходе из иму, т.к. велосити шумит ~ в диаппазоне +/-10…15
Не, ни в коем случае. Если она шумит вокруг 0, то в сумме и так управляющее воздействие 0. Да и работать она должна именно вокруг 0, т.к. по alt у нас все плавно (если исключить “тестовые” рывки “на камеру” 😃 А deadband внесет такую нелинейность что последсвия трудно даже спрогнозировть…
Стэнд - это круто 😃 Ме хватило рук, чтобы висение отладить 😃 Я думаю они к таким же резултатм придут: при висении точность определит только барометр. А вот при полетах, вот тут сложнее и инетреснее. И стэнд особо не поможет 😃
Судя по видео и резким скачкам вверх вниз без видимой прчины, пока не все гладко 😃
Ещё сделал мелкую доработку: коррекция газа при наклоне аппарата THROTTLE_ANGLE_CORRECTION, чтобы вертикальная составляющая осталась неизменной. Это должно помочь не терять высоту при начале движения. Работает только в stable mode. Возможно придется умножить на понижающий коэффициент, т.к. не уверен в линейной зависимости газа и косинуса угла.
Код на SVN code.google.com/p/multiwii-alexmos/…/MultiWii/, НЕ ТЕСТИРОВАЛСЯ.
Попробовал такой алгоритм:
acc_delta_alt = 9.8 * (az/LSB -1) * cycleTime * cycleTime; // 9.8 * az [м\с2] * dT *dT =acc_delta_alt [ м ]
Altitude = 0.85f * Altitude + 0.15f * alt_baro + acc_delta_alt;
Фигня получается. Высота скачет как и раньше с баро_только. И приращения acc_delta_alt, похоже, частично теряются, не успевает складывать…
9.8 * az [м\с2] * dT *dT
это у вас не двойное интегрироавние, потому и высота не выходит. И так просто как вых написали, проблему не решить 😃
Это я применил небольшую оптимизацию, чтобы не дублировать “тяжелы” умноженя и деления. На самом деле твоя фомула и моя совершенно одинаковы, это в обоих случаях комплементарный фильтр. Я проверил, одна формула сводится к другой раскрытием скобок.
то что и там и там CF это понятно… раскрыл скобки… в оптимизированной версии результат получатся меньше на alt/ACC_BARO_CMPF, который в след-й итерации увеличит ошибку на эту величину по идее
ALT = alt *(1.0f - 1.0f/ACC_BARO_CMPF) - err =
alt *(1.0f - 1.0f/ACC_BARO_CMPF) - (alt - sensorAlt)/ACC_BARO_CMPF =
alt - alt/ACC_BARO_CMP - alt/ACC_BARO_CMP + sensorAlt/ACC_BARO_CMPF =
(alt*ACC_BARO_CMP - alt*2 + sensorAlt)/ACC_BARO_CMPF=
(alt*(ACC_BARO_CMP - 2) + sensorAlt)/ACC_BARO_CMPF
что равносильно (alt*(ACC_BARO_CMP - 1) + sensorAlt)/(ACC_BARO_CMPF + 1)
Пока трудно представить ситуацию, чтобы ускорение “резко” рвануло при удержании высоты. А при плавных возмущенях, каковые в основном и наод держать - шумом в ускорении все же нельзя пренебрегать, он становится сравним с измеряемой величиной. Поэтому я и против привлечения ускорения в PID-регулятор - только лишние дергания для моторов. В удержании угла то же самое - если ставить высокий D на ROLL и PITCH - начинается кобасня на выходах мотров. И хотя это незаметно и они по прежнему стабилизируют хорошо, все же мне кажется что текущее решене неправильное - не нужно учитывать ускорение в PID регуляторе. И по теории не нужно, и логика это подсказывает.
я тоже сперва так думал и писал что по логике приведет к осцилляциям… по ссылкам выше размышлизмы один в один rcopen.com/forum/f123/topic265409/9
за “Д” они принимают дифференциал угловой скорости - т.е. угловое ускорение, как для акро так и для стаб.мода., а должен быть на крайняк дифференциал изменения угла - т.е. угловая скорость… Вывел графики ПИД-ов в ГУИ, занулил “П” и “И”, так и есть “Д” у них - это угловое ускорение… В итоге по теории это может привести лишь к лишним осциляциям, т.к. это добавит компенсацию/всплеск не только в нужную полярность, а также и в противоположную…
соот-но попробовал убирать PD (по скорости) и добавлять D (по углу) к PI в различных вариациях (усредненное 3-х последних скоростей; либо принимая угловую скорость гиры за D и т.д ) rcopen.com/forum/f123/topic221574/3003
сам тестил + помоШника нашел (хороший пилот)… не полетело в итоге… раскачки, осцилляции, ломанные винты…
В теории просто ПИД хорош беспорно и главное понятен, а вот на практике не хватает его для устойчивой стабилизации ЛА, потому и используют комплексные ПИД регули думаю…
Попробуй сделать. Вполне допускаю, что комплементарный фильтр не самый лучший для барометра. У меня времени нет сейчас, и не охота пока усложнять алгоритм - уже и так слишком много теории, хочется попрбовать сначала, что текущее решение дает.
ну я говорил не про замену CF на медиан, а про предварительную фильтрацию данных с барометра медианом или для начала обычный НЧ попробовать поставить…
в оптимизированной версии результат получатся меньше на 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 миллисекунд.