MultiWii - обсуждаем и отлаживаем Alt Hold
Решил перенести обсуждение из ветки multiwii (начальное сообщение по этой теме - rcopen.com/forum/f123/topic221574/4475
habrahabr.ru/blogs/arduino/137595/ Использование инерциальной навигационной системы (ИНС) с несколькими датчиками на примере задачи стабилизации высоты квадрокоптера
У вас с включенными моторами акселерометр не глючит (переполнение АЦП)?
Я BMA180 настроил на 8G, 40Hz ФНЧ.
Кстати, когда коптер движется ускоренно или по криволинейной траектории, то акселерометр дает больше\меньше, чем 1g ( sqrt(ax*ax+ay*ay+az*az) <> 1g*LSB ). Я пробовал компенсировать “лишний” вектор ускорения за счет скорости по GPS: ax -= omega_z * gps_speed_y; az += omega_x * gps_speed_y; Но тут косяк - предполагается, что ЛА летит осью Y вперед, и никак иначе. Но квадрик может двигаться боком (осью Х). А скорость по GPS не проецируется на конкретную ось.
попробую объяснить почему я откзалася от раздельных PI и PD. D в высоте - это производная ошибки, но данные о высоте у нас неточные и D от этих даных будет ещё хуже. Но производная ошибки - это и есть скорость (dErr = d(Hhold - H) = dHhold - dH = -dH), которую наш алгоритм худо-бедно выдает. То есть я просто обозвал P-составляющую скорости D-составляющей высоты, не изменив логики оригинальной версии, только PID высоты теперь стал классическим PID.
pogodi… esli ishodnie dannie eto visota, to v terminah PID regulyatorov err eto izmenenie visoti, t.e. skorost’ za dt cikla…
err=h_hold-h
a vot D sostavlyauschaya eto uzhe skorost’ izmeneiya oshibki, t.e. uskorenie:
D = dt(err - prev_err)
primer: h0=0, h1=5, h2=12
err1=5-0=5 (skorost’ izmeneiya visoti za dt), err2=12-5=7 (skorost’ izmeneiya za dt)
D=(7-5)=2 (uskorenie za dt)
t.e. eto uzhe ne klassik PID, a mix
dlya spravki moih rassuzhdeniy pidcontrol.narod.ru
А VelocityPID остался только D - производная скорости, что по сути является ускорением. А так ли нужен этот компонент? Посмотри на график ускорения при любом линейном перемещении - это выброс вверх и равнозначный выброс вниз. Т.е. компенсация а короткий перод времени даст +100 а потом -100 - такое моторы даже не успеют отработать, а есть ещё инерция коптера. Так что смысла от D-скорости ноль.
esli rassmotret’ na primere wii level mode, to tam D eto kak raz uglovoe uskorenie, pravda usrednennoe za poslednie tri iteracii:
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
I ya tozhe dolgo ne mog ponyat’ zachem eto nuzhno esli vspleski simmetrichnie, no potom kazhetsya doper. Oni to simmetrichnie, NO ne po amplitude a po ploschadi, t.e. na razgon mozhet bit’ narastayuschiy pologiy grafik, a na tormozheniye krutoy rezkiy pik, libo krutoy na razgon, no pologiy na tormozhenie… i glavnoe chto v level mode eto otlichno reguliruet skorost’ raboti sistemi…
Александр, пользуйтесь пожалуйста транслитпереводчиком www.translit.ru а то мозг сломать можно.
to v terminah PID regulyatorov err eto izmenenie visoti, t.e. skorost’ za dt cikla
Нет, P - это ПРОПОРЦИОНАЛЬНАЯ составляющая, т.е. это и есть ошибка, без dT. P = err * kP, а D - это ДИФФЕРЕНЦИАЛЬНАЯ, D = derr/dt * kD
esli rassmotret’ na primere wii level mode, to tam D eto kak raz uglovoe uskorenie, pravda usrednennoe za poslednie tri iteracii
Да, потому что в данном случае (в акрорежиме) стабилизируется скорость, а для нее D - это ускорение. Мы же стабилизируем высоту, а для нее D - это скорость.
У вас с включенными моторами акселерометр не глючит (переполнение АЦП)? Я BMA180 настроил на 8G, 40Hz ФНЧ
Нет, не глючит. У меня хорошая виброразвязка, но я тоже включил 8G (его включили в версии 1.9, я просто взял последние изменения). Для моего алгоритма переполнений по оси Z быть не должно, это важно!
Я пробовал компенсировать “лишний” вектор ускорения за счет скорости по GPS
Господи, зачем такие сложности?? Просто спроектируйте его на вектор ориентации (у меня в статье этот момент расписан). У вас останется только вертикальное ускорение при любой ориентации аппарата и любом горизонтальном ускорении. Есть пара тонкостей: я в своих исходниках более точно выставлил 0 гир и уменьшил влияние на них акселеромтера, чтобы ориентация была как можно точнее.
Александр, пользуйтесь пожалуйста транслитпереводчиком www.translit.ru а то мозг сломать можно.
polzovalsya ranee, no za neimeniem vremeni na rabote i chtobi menshe svetit’ raznocvetnim ekranom translit.ru budo postit’ tak… potomu sorry ))
Да, потому что в данном случае (в акрорежиме) стабилизируется скорость, а для нее D - это ускорение. Мы же стабилизируем высоту, а для нее D - это скорость.
ya pisal pro stab/level mode s ego PI-PD regulyatorom, gde konechnaya cel’ stabilizaciya ugla, v chem PD regulyator s ishodnoy “uglovaya skorost” kak raz i daet uverennuyu stabilizaciyu… libo visoti v sluchae 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 на медиан, а про предварительную фильтрацию данных с барометра медианом или для начала обычный НЧ попробовать поставить…