Создание собственной системы стабилизации
всем привет. Кто может просветить по поводу ПИДов. Вот две схемы ПИДов танагажа и крена.
Первая - паралелльная, довольно странная, это схема автоквада. Причем автор хотел сделать, не то, что получилось? Ошибка? Обратите на то, что из сигнала задания в звене дифференциальной состяавляющей вычитается СНОВА показание сенсора. Наверное надо вычитать ЗАДАНИЕ? Так как коментарий противоположен, выделил жирным.
Вторая - моя текущая. Она последовательная. Я вычитаю 80 процентов задания, по-моему так правильнее.
В схемах намеренно не показывал лимиты, чтобы не загромождать.
Но к чему это я все. На форуме автоквада написано, что на заре проекта, было несколько схем (последовательная и паралелльная), в итоге Билл выбрал паралелльную, так как она оказалось лучше. forum.autoquad.org/viewtopic.php?f=31&t=4792&p=363…
Но вот смотрю я на нее и с первого взгляда, что-то нифига не лучше, вместо того, чтобы использовать напрямую угловую скорость с гиры в ПИД идет дифференциал угла. У кого какие мысли?
Код:
float pidUpdate(pidStruct_t *pid, float setpoint, float position) {
float error;
float p = *pid->pGain;
float i = *pid->iGain;
float d = (pid->dGain) ? *pid->dGain : 0.0f;
float f = (pid->fGain) ? *pid->fGain : 1.0f;
if (pid->pTrim)
p += (*pid->pTrim * p * 0.002f);
if (pid->iTrim)
i += (*pid->iTrim * i * 0.002f);
if (pid->dTrim)
d += (*pid->dTrim * d * 0.002f);
if (pid->fTrim)
f += (*pid->fTrim * f * 0.002f);
error = setpoint - position;
// calculate the proportional term
pid->pTerm_1 = p * error;
if (pid->pTerm_1 > *pid->pMax) {
pid->pTerm_1 = *pid->pMax;
}
else if (pid->pTerm_1 < -*pid->pMax) {
pid->pTerm_1 = -*pid->pMax;
}
// calculate the integral state with appropriate limiting
pid->iState += error;
pid->iTerm_1 = i * pid->iState;
if (pid->iTerm_1 > *pid->iMax) {
pid->iTerm_1 = *pid->iMax;
pid->iState = pid->iTerm_1 / i;
}
else if (pid->iTerm_1 < -*pid->iMax) {
pid->iTerm_1 = -*pid->iMax;
pid->iState = pid->iTerm_1 / i;
}
// derivative
if (pid->dGain) {
// uncomment this line if you want the D term to ignore set point changes
error = -position;
pid->dTerm_1 = (d * f) * (error - pid->dState);
pid->dState += f * (error - pid->dState);
if (pid->dTerm_1 > *pid->dMax) {
pid->dTerm_1 = *pid->dMax;
}
else if (pid->dTerm_1 < -*pid->dMax) {
pid->dTerm_1 = -*pid->dMax;
}
}
else {
pid->dTerm_1 = 0.0f;
}
pid->pv_1 = position;
pid->sp_1 = setpoint;
pid->co_1 = pid->pTerm_1 + pid->iTerm_1 + pid->dTerm_1;
if (pid->co_1 > *pid->oMax) {
pid->co_1 = *pid->oMax;
}
else if (pid->co_1 < -*pid->oMax) {
pid->co_1 = -*pid->oMax;
}
return pid->co_1;
}
Кто может просветить по поводу ПИДов.
Не совсем понял, у тебя на схемах где вход от ДУС?
справа RATE - это данные с ДУСА
Код плохо смотриться, исправь с [CODE]
Мне твой вариант больше нравится, но я бы сделал не много по другому.
Дифф составляющую я бы вообще отдельной параллельной веткой сделал от скорости и отвязал от задатчиков по скорости и тем более углу. Тогда вот эта ветка с весом 0.8 уберется. Задача дифф ветки "успокоить переходные процессы регулирования угла.
Тогда вот эта ветка с весом 0.8 уберется.
Я понял идею. Но мне именно был интересен вариант с тем, чтобы на резкое изменение задания коптер тоже резко реагировал, поэтому 20 процентов сигнала задания я подмешиваю в дифференциальную составляющую.
Мне твой вариант больше нравится
Вот Билл по-другому решил. Единственное что мне пока понравилось в паралелльной схеме это то, что коэффициенты контура угловой скорости независимы от коэффициентов контура угла, то есть по идее, на разных коптерах перенастройку делать легче или вообще не надо. Или все таки не легче и надо?
Илья, у тебя отсутвует интегральная часть на Angle. Если действует постоянная ошиба на rate, коптер никогда не займет заданного положения по углу. В схеме автоквада она есть. Но схема автоквада все равно очень странная, возможно, ее можно преобразовать к классической двухкаскадной, но лучше уже сразу от классической и идти, как у тебя.
Спасибо за интересное замечание, я об этом не задумывался. Хотя такой ситуации быть не должно, у меня на ПИД поступает скорость с ДУСа+биас (который калман считает, то есть все время обновляется), поэтому скорость поступает без смещения. Но действительно можно добавить еще интегральное звено по углу.
чтобы на резкое изменение задания коптер тоже резко реагировал, поэтому 20 процентов сигнала задания я подмешиваю в дифференциальную составляющую.
На мой взгляд это пустой расход энергии, если регули позволят, а если не позволят, то они это вообще пропустят. ДИФФ-звено это демпфер от раскачки и больше ничего.
Илья, у тебя отсутвует интегральная часть на Angle. Если действует постоянная ошиба на rate, коптер никогда не займет заданного положения по углу.
Алексей, есть у него интеграл, просто он через два коэффициента TLT_ANG_P*RTE_ANG_I.
Но действительно можно добавить еще интегральное звено по углу.
Ничего не надо, будет медленная раскачка, двойной интеграл вносит большую задержку - очень тяжело настраивать. А в данном случае и не нужно.
На мой взгляд это пустой расход энергии, если регули позволят, а если не позволят, то они это вообще пропустят. ДИФФ-звено это демпфер от раскачки и больше ничего.
Тут позволю не согласиться. Дифф-компонента ускоряет реакцию контроллера на изменение цели. Должна быть возможность контролировать вес цели в Дифф-компоненте, на некоторых сетапах это способно значительно ускорить реацию на команду.
Ничего не надо, будет медленная раскачка, двойной интеграл вносит большую задержку - очень тяжело настраивать. А в данном случае и не нужно.
Согласен, особенно если дрейф гироскопа рассчитан и учтен фильтром.
На мой взгляд это пустой расход энергии, если регули позволят, а если не позволят, то они это вообще пропустят. ДИФФ-звено это демпфер от раскачки и больше ничего
Не соглашусь, ничего не пропустят, дифф часть через ФНЧ проходит, поэтому там не будет скачка, а вполне растянутый горбик, нормально регули отыграют. Это не только демпфер, но и ускоритель для быстрого отработки задания. К тому же в зависимости от того, как резко должен коптер реагировать на изменение задания отвечает этот самый коэффициент, который у меня равен 0,8. Сделать меньше - будет резче, сделать больше - коптер будет вести себя как вы хотите.
лексей, есть у него интеграл, просто он через два коэффициента TLT_ANG_P*RTE_ANG_I.
Интеграл то есть, но как Алексей сказал, если угловая скорость смещена, то есть ненулевая, при нулевой фактической скорости, то коптер никогда в горизонт не встанет, поэтому он предложил добавить еще интегральное звено в угловой контур.
Ничего не надо, будет медленная раскачка, двойной интеграл вносит большую задержку - очень тяжело настраивать. А в данном случае и не нужно.
Вот тут да, есть такой момент, вроде как из ТАУ вспоминаю, что запас устойчивости снижается. Но зато астатизм системы повышается.
Но все равно фиг с ним с этим интегралом, главное подавать несмещенные угловые скорости, тогда все нормально будет, а у меня так и есть.
Не соглашусь, ничего не пропустят, дифф часть через ФНЧ проходит, поэтому там не будет скачка, а вполне растянутый горбик, нормально регули отыграют. Это не только демпфер, но и ускоритель для быстрого отработки задания. К тому же в зависимости от того, как резко должен коптер реагировать на изменение задания отвечает этот самый коэффициент, который у меня равен 0,8. Сделать меньше - будет резче, сделать больше - коптер будет вести себя как вы хотите.
Делать-не делать, тут дело вкуса 😃 По мне (ещё раз повторюсь) смысл этого ДИФФ-звена - компенсация (демпфер) колебаний в следящей системе, вызванный фазовым сдвигом из-за задержки в цепи среда-обработка-реакция (угловая скорость-датчик-контроллер-ВМГ-угловая скорость). Быстродействия хватает и без него.
если угловая скорость смещена, то есть ненулевая, при нулевой фактической скорости, то коптер никогда в горизонт не встанет, поэтому он предложил добавить еще интегральное звено в угловой контур.
Для того чтоб два интегральных звена работали последовательно надо иметь отрицательную обратную связь от средней точки (между ними), т.е. по угловой скорости. Но это и так делается, только в интеграторе (прогнозе) ФК.
у меня на ПИД поступает скорость с ДУСа+биас (который калман считает, то есть все время обновляется), поэтому скорость поступает без смещения.
Зачем ещё одно звено?
Но все равно фиг с ним с этим интегралом, главное подавать несмещенные угловые скорости, тогда все нормально будет, а у меня так и есть.
вроде как из ТАУ вспоминаю
Коллега…
поэтому 20 процентов сигнала задания я подмешиваю в дифференциальную составляющую.
Мне ТАУ вспомнить в два раза сложнее )), но попробую тоже высказаться на этот счет - классический ПИД самодостаточен, все желаемые характеристики типа “скорости реакции” и т.п. достигаются тупо коэффициентами, а всякого рода “подмешивания” это скорей самообман… (ИМХО), т.е. у нас есть классическая “уставка” и “данные сенсора”, ладно, если подмешивать “нечто - третье”, а так - ходьба по кругу…
(возможно мой тезис можно было бы математически расписать, но уже слабо…)))
А на мой взгляд, всё не так
Я когда-то упоминал перевернуть ПИД с ног на голову и использовать в качестве пропорции самую точную часть показаний - угловую скорость, сейчас смотрю рейсеры как раз таким ПИД-ом и пользуются…
в качестве пропорции самую точную часть показаний - угловую скорость,
Ну это скорей к ПИД-у, как таковому, отношения не имеет, это просто замена одной величины регулирования на другую…,
, если я правильно понял идею - конечно, если углы из уг.скоростей не считать, то быстродействие регулятора растет.
если углы из уг.скоростей не считать, то быстродействие регулятора растет.
угол как раз будет интегральной составлящей, а диф - ошибка угловой скорости… можно конечно заморочится и высчитать ускорение… ещё одна бредовая мысль - с акселя взять центростремительное ускорение? оно ж вдоль осей…
ещё одна бредовая мысль - с акселя взять центростремительное ускорение? оно ж вдоль осей…
Везде шумы шумы и шумы, я не знаю как с ними бороться, а начинаешь бороться, все быстродействие насмАрку…
Всем привет! Запилил новые 2 режима для фана.
Еще ко мне Dji Phantom 3 Advanced пришел. Первые впечатления, что довольно продуманы некоторые ньюансы. Векторного управления на движках я не заметил ,по-моему FOC должен себя иначе вести, но я не уверен. У подвеса обратная связь по положению ротора есть, отклонения отрабатывает плавно, без дребезга. Попробовал погазовать без винтов - коптер видит, что винтов нет, выводит сообщения. По яву - реагирует на угловую скорость, а не на положение. Однако в полете возвращается по яву.
Вообщем надо изучать. Удержание позиции работает четко и быстро, не плавает.
Будет время запилю тесты, сравнение.
Еще в планах попробовать отказаться от компаса вовсе и реализовать задумку с 2-мя ЖПС на диагонали для определения ориентации. Но это попозже
новые 2 режима
Как выглядят критерии “попадания” в заданную точку ?
Проверяется попадание текущего положения в некий квадрат или радиус ??
да, проверяется расстояние от текущего положения коптера до указанной точки, если оно меньше 0,4 м, то переходим в следующую точку. 0,4 м просто взял на глаз.
По идее сфера радиусом 0,4 м получается.
0,4 м, то переходим в следующую точку.
А курс на точку, я так понимаю, считается и корректируется постоянно в течении следования (?)…
Вообще, нельзя ли подсмотреть функцию пересчета положения по GPS в азимут точки ?? Для сравнения со своей:
int16_t get_azimut( float lat1, float long1, float lat2, float long2)
{
float dlon_W, dlon_E,dphi, atn2,tc;
int16_t az;
int8_t sign;
dlon_W = (long2 - long1) - (2*PI*(floor((long2 - long1)/(2*PI))));
dlon_E = (long1 - long2) - (2*PI*(floor((long1 - long2)/(2*PI))));
dphi = log((tan((lat2/2) + (PI/4)))/(tan((lat1/2) + (PI/4))));
if (dlon_W < dlon_E) {
dlon_W = -1*dlon_W;
if (dlon_W >= 0)
sign = 1;
else
sign = -1;
if (abs(dlon_W) >= abs(dphi)) {
atn2 = (sign * PI/2) - atan(dphi / dlon_W);
}
else {
if (dphi > 0) {
atn2 = atan(dlon_W / dphi);
}
else {
if ((-1*dlon_W) >= 0) {
atn2 = PI + atan(dlon_W / dphi);
}
else {
atn2 = (-1*PI) + atan(dlon_W / dphi);
}
}
}
}
else {
if (dlon_W >= 0)
sign = 1;
else
sign = -1;
if (abs(dlon_E) >= abs(dphi)) {
if (dlon_E > 0)
atn2 = sign * PI/2 - atan(dphi / (dlon_E));
else
atn2 = 0;
}
else {
if (dphi > 0) {
atn2 = atan((dlon_E) / dphi);
}
else {
if ((dlon_E) >= 0) {
atn2 = PI + atan((dlon_E) / dphi);
}
else {
atn2 = (-1*PI) + atan((dlon_E) / dphi);
}
}
}
}
tc = atn2 - (2*PI*(floor((atn2)/(2*PI))));
az=tc*toGRAD;
if(az<0){az+=360;}
if(az>359){az=0;}
return az;
}
Если вы про курс носа коптера, то он вообще отвязан, я им управляю с пульта.
У меня при первом хорошем приеме ЖПС высчитываются коэффициенты для пересчета из градусов долготы и широты в метры. Далее все считается в метрах от старта
Вот это обработка в прерывании по приему GPS:
if ((GPS_satellits > 8)&&(GPS_pDop<175))
{
if (flag_start_home == 0)
{
double LAT;
flag_start_home = 1;
GPS_height_start = GPS_height;
GPS_lon_start = GPS_lon;
GPS_lat_start = GPS_lat;
LAT = (double)GPS_lat/10000000.0f;
navUkfCalcEarthRadius(LAT, &KX, &KY);
}
xSemaphoreTake(xGPS_UKF_Mutex, portMAX_DELAY);
GPS_X = ((float)(GPS_lon-GPS_lon_start))*KX;
GPS_Y = ((float)(GPS_lat-GPS_lat_start))*KY;
GPS_alt = ((float)(GPS_height-GPS_height_start))*0.001f;
xSemaphoreGive(xGPS_UKF_Mutex);
StateNavUKF &= ~UKF_BAD_GPS;
flag_get_pvt = 1;
}
сама функция персчета, взята у автоквада:
static void navUkfCalcEarthRadius(double lat, float *k_x, float *k_y) {
double sinLat2;
sinLat2 = sin(lat * (double)DEG2RAD);
sinLat2 = sinLat2 * sinLat2;
*k_y = (double)NAV_EQUATORIAL_RADIUS * (double)DEG2RAD * ((double)1.0 - (double)NAV_E_2) / pow((double)1.0 - ((double)NAV_E_2 * sinLat2), ((double)3.0 / (double)2.0));
*k_x = (double)NAV_EQUATORIAL_RADIUS * (double)DEG2RAD / sqrt((double)1.0 - ((double)NAV_E_2 * sinLat2)) * cos(lat * (double)DEG2RAD);
*k_y /=10000000.0f;
*k_x /=10000000.0f;
}