Пост фильтр для устранения дребезга сервомашинок.

Данные IMU датчиков после прохождения фильтров и закона управления обладают остаточным шумом 0.5 -1 град из которого сервомашинки сильно дребезжат. Не хочется применять скользящее среднее или экспоненту т.к. это замедлит реакцию, (которая и без того медленная из за дельта Е / дельта Т). Каким образом можно избежать дребезга и не сильно уменьшить время реакции?

 

Что за IMU? Что за фильтры? Какой закон? Что за серво? Где модель цепи? Где схема?

 

IMU 9250 10DOF, фильтр Маджвика, закон управления от АБСУ Ту 154 ( σ = кυ(υ – υз) + кυрυ + (кн+кни/р)·(Н-Нз) - кγ|γ| ), цифровая серва на 20 кг работает нормально, схема очень простая от источника питания 6.8В 10А питание, общий провод и ШИМ на DUE при ручном управлении всё работает нормально даж выдаёт около 20кг с норм проводами.

Если полностью расписывать будет целая статья, вопрос в том как сделать шумодав что-бы он был оптимален по быстродействию.

 

Ну шутим так шутим. Удачи.

 

Оптимизировать код, а потом ввести усреднение?

 

Никаких шуток, если что-то есть смешное напишите.

Это я и собираюсь сделать вопрос каким усреднением воспользоваться .Взять тот-же Pixhawk даже лучшие образцы фильтров Калмона обладают остаточным шумом около 0.7 град (если это касается ориентации) но подавать сигнал с таким шумом после закона управления на сервомашинку нельзя будет дребезг но как-то же там там это решено?

Вероятно значения усредняются но тогда это замедлит реакцию т.к. скользящая средняя предполагает обработку массива где каждое новое значение замещает самое старое а потом на основании значений находится средняя. Так вот на замещении уходит несколько тактов фильтра, что приводит к задержке а это в свою очередь в сложности настройки ПИД а иногда и невозможности его настроить. Вопрос как отфильтровать значения так что-бы использовать минимальное кол-во тактов фильтра?

З.Ы. Закон управления по сути ПИД где в место обычный дельты используется проекция угловой скорости на связанную систему координат что-бы отфильтровать побочную угловою скорость это в свою очередь позволяет избежать отклонения рулей при девиации от значений своей оси т.к. например при тонгаже 90 град ось крена заменяется рысканьем и при крене нужно постепенно считывать сигнал уже с оси рысканья что-бы определить реальную угловую скорость вдоль оси этакая перекрёстная связь. Изначально не правильно прочёл коммент.

 

А если округлять до целых градусов?

 

Уже округляю, пробовал сделал экспоненту т.к. она выполняется быстрее ( pitch = Last_pitch + 2 * ( pitch - Last_pitch ) / (1 + n) ) но при малых значениях почти нет эффекта при больших тоже долго.

 

DMP используется? Код для DMP сами делали или взяли готовый?
Ну и фильтры надо смотреть на счёт оптимизации.

Это понятно. К тому же, с одной стороны целую статью вряд ли кто читать будет, а с другой -- без неё советов полезных не будет.

 

т.е. теперь дребезг +-1 градус? (в первом посте было меньше)

чем что? чем округление?
С какими типами вы работаете? с целыми? то тут все операции занимают 1 такт. Доставайте значения из датчика в кодах, усредняйте, обрабатывайте, а потом переводите к нужному вам виду. Усреднение в интах делать легко, выбираем кол-во усредняемых величин кратное двойке и тогда деление на 2^х эквивалентно сдвигу числа вправо x раз (Y/2^x == Y>>X).

Ещё вариант. Честно говоря, забыл как называется такой тип усреднения. Опишу его:
avr = предыдущее среднее значение (avr - от average)
...
avr_new = (avr*(n-1) + X) / n
обработать, использовать
...
avr = avr_new

Т.е. тут считается что среднее из n-1 элемента уже посчитано, осталось посчитать среднее с учётом нового текущего значения с датчика.
У этого усреднения будет переходной процесс при включении, т.к. с самого начала avr = 0 (или другое нач. значение, но точно не истинное).
Его преимущество в том, что нет проблем работы с массивом.

 

Да используется, калибровочные заводские данные и в самом фильтре Маджвика ошибку градиентного спуска и перевода в кватернионы тоже исправил. Это по сути это динамическая ошибка которая есть всегда, наверняка коммерческие автопилоты её как-то просто давят об этом даже не пишут.

 

Коммерческие автопилоты используют совсем другое железо, а не общедоступные MPU9250.

Хороший ответ, вот прям всё ясно стало как используется.

 

Получается что так, завтра выведу всё на графики, т.к. на данный момент всё отображается через в терминале там ошибку не заметно число (допустим 40) статично но серва дребезжит, в ручном управлении такого нет.

Чем скользящая среднее и прочее до неё, изначально я перевожу кватернион в число с плавающей точкой float для перевода из радианов в градусы, я пробовал использовать целочисленное значение или trunc() как я уже писал в терминале ошибка не видна.

Вот более полный ответ:

Спойлер: Код инициазизации датчика

Код (C++):

void initMPU9250()
{
// wake up device
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
  delay(100); // Wait for all registers to reset

// get stable time source
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_1, 0x01);  // Auto select clock source to be PLL gyroscope reference if ready else
  delay(200);
 
// Configure Gyro and Thermometer
// Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
// minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
// be higher than 1 / 0.0059 = 170 Hz
// DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
// With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
  writeByte(MPU9250_ADDRESS, MPU9250_CONFIG, 0x03);

// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
  writeByte(MPU9250_ADDRESS, MPU9250_SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate
                                    // determined inset in CONFIG above
// Set gyroscope full scale range
// Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  uint8_t c = readByte(MPU9250_ADDRESS, MPU9250_GYRO_CONFIG);
//  writeRegister(GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
  writeByte(MPU9250_ADDRESS, MPU9250_GYRO_CONFIG, c & ~0x03); // Clear Fchoice bits [1:0]
  writeByte(MPU9250_ADDRESS, MPU9250_GYRO_CONFIG, c & ~0x18); // Clear GFS bits [4:3]
  writeByte(MPU9250_ADDRESS, MPU9250_GYRO_CONFIG, c | MPU9250Gscale << 3); // Set full scale range for the gyro
// writeRegister(GYRO_CONFIG, c | 0x00); // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
 
// Set accelerometer full-scale range configuration
  c = readByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG);
//  writeRegister(ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
  writeByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
  writeByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG, c | MPU9250Ascale << 3); // Set full scale range for the accelerometer

// Set accelerometer sample rate configuration
// It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
// accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
  c = readByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG2);
  writeByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
  writeByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz

// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
// but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting

  // Configure Interrupts and Bypass Enable
  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
  // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
  // can join the I2C bus and all can be controlled by the Arduino as master
   writeByte(MPU9250_ADDRESS, MPU9250_INT_PIN_CFG, 0x22);  
   writeByte(MPU9250_ADDRESS, MPU9250_INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt
   delay(100);
}


// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void accelgyrocalMPU9250(float * dest1, float * dest2)
{
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
 
// reset device
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
  delay(100);
 
// get stable time source; Auto select clock source to be PLL gyroscope reference if ready
// else use the internal oscillator, bits 2:0 = 001
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_1, 0x01);
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_2, 0x00);
  delay(200);                                  

// Configure device for bias calculation
  writeByte(MPU9250_ADDRESS, MPU9250_INT_ENABLE, 0x00);   // Disable all interrupts
  writeByte(MPU9250_ADDRESS, MPU9250_FIFO_EN, 0x00);      // Disable FIFO
  writeByte(MPU9250_ADDRESS, MPU9250_PWR_MGMT_1, 0x00);   // Turn on internal clock source
  writeByte(MPU9250_ADDRESS, MPU9250_I2C_MST_CTRL, 0x00); // Disable I2C master
  writeByte(MPU9250_ADDRESS, MPU9250_USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
  writeByte(MPU9250_ADDRESS, MPU9250_USER_CTRL, 0x0C);    // Reset FIFO and DMP
  delay(15);
 
// Configure MPU6050 gyro and accelerometer for bias calculation
  writeByte(MPU9250_ADDRESS, MPU9250_CONFIG, 0x01);      // Set low-pass filter to 188 Hz
  writeByte(MPU9250_ADDRESS, MPU9250_SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz
  writeByte(MPU9250_ADDRESS, MPU9250_GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
  writeByte(MPU9250_ADDRESS, MPU9250_ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g

// Configure FIFO to capture accelerometer and gyro data for bias calculation
  writeByte(MPU9250_ADDRESS, MPU9250_USER_CTRL, 0x40);   // Enable FIFO
  writeByte(MPU9250_ADDRESS, MPU9250_FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)
  delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes

// At end of sample accumulation, turn off FIFO sensor read
  writeByte(MPU9250_ADDRESS, MPU9250_FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
  readBytes(MPU9250_ADDRESS, MPU9250_FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
  fifo_count = ((uint16_t)data[0] << 8) | data[1];
  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
 
  for (ii = 0; ii < packet_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
    readBytes(MPU9250_ADDRESS, MPU9250_FIFO_R_W, 12, &data[0]); // read data for averaging
    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;  
    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
   
    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
           
}
   // Далее выложить лимит не позволяет
}

 

Нужно выводить всё.
Что такое ручное управление, и чем оно отличается от неручного?
Предположу что в ручном вы выставляете серву в какое-то положение и больше не трогаете, а в неручном постоянно долбите одним и тем же значением.
Будет дребезг если серву долбить одним и тем же значением?

 

Похоже мы друг друга не очень понимаем...
Оговорюсь сразу, я не очень хорошо разбираюсь в данных аксельрометрах. Что успел узнать в контексте MPU-6050 (младший брат MPU-9250): в данных контроллерах есть Digital Motion Processor, который может производить первичную обработку данных, получаемых с аксельрометров. Но для активации этой штуки нужно залить прошивку в MPU-6050 (наверно можно читать как MPU-9050), которая как раз и будет что-то там обсчитывать. Прошивка для MPU-6050 весит около 2 кБайт. Что за прошивка -- х.з. Для компиляции такой прошивки есть "Motion Apps" -- https://www.invensense.com/motion/. Но простым смертным как я она не доступна -- или я плохо искал.

И теперь по поводу ответа на вопрос -- "DMP используется?":

Вы уверены, что используете DMP? Т.к. в ответе:

не видно загрузки какой-либо прошивки в MPU-9050.

Ещё раз повторю -- мои знания в этом вопросе поверхностные.

 

Это не весь код полностью уместить не удалось, (как я пересмотрел тут только сброс регистров DMP перед отправкой) если под DMP вы подразумеваете встроенный алгоритм фильтрации движений то он должен использоваться. ( Код инициализации состоит из разных кусков кода которые я подсмотрел в других решениях поэтому сходу точно не вспомню как именно это работает ).

Но если вернутся к теме то DMP не решит проблему, потому, что точность фильтра в принципе приемлемая (На сколько я помню в книге БПЛА (изд. Машиностроение за 1967) сказано, что точности в 30град достаточно для автопилотов при наличии ВЦУ а в данном случае GPS) в моём случае точность 1.5 град, допустим DMP не использовался, после его добавления точность увеличится до 0.5 град это всё равно не решит проблему дребезга сервы т.к. в динамической системе колебания будут усиливаться (да ещё и двигатель добавит) . Поэтому можно считать что шум от фильтра является флуктуационным, кстати датчик чувствителен к температуре.
Результаты работы любого фильтра должны сглаживаться пост фильтром что-бы колебания в несколько градусов не приводили к дребезгу но применение обычных алгоритмов в системе с обратной связью приводит к задержкам, что в дальнейшем усложнит настройку, алгоритм должен обладать минимальной задержкой и вот тут проблема в том, что большинство алгоритмов интегрируют данные, что не приемлемо для обратной связи.

 

ув. ТС, вы же понимаете, что нельзя сказать "такой-то алгоритм не подходит, т.к. в него заложено интегрирование"? Ведь, у алгоритмов несколько параметров, по которым выбирают нужный.

Вам уже написали про оптимизацию не один раз. Вы этот момент игнорируете.

 

Это ещё почему?

 

Я просто ответил на вопрос, что-бы в дальнейшем разговор про DMP не вводил в заблуждение, это конечно не отменяет оптимизацию, я непременно это сделаю.

Потому, что интеграл увеличивает задержку, тем самым увеличивая автоколебания, это дестабилизирует систему. А я напомню что в отличии от АБСУ а меня нет датчиков угловых скоростей (ДУС) поэтому приходится использовать производную, а это тоже задержки, конечно для небольшого ЛА они не значительны т.к. он не обладает такой инерцией но интегральная составляющая вносимая пост фильтром по сути будет инерцией системы.

 

Что-то не то. Не хватает реакции системы - необходимо оптимизировать код или менять МК. Увеличение постоянной времени интегратора никак не может привести к увеличению автоколебаний. Амплитуду автоколебаний может увеличить дифференциальная цепь, когда желая увеличить реакцию системы на входное воздействие, мы через чур увеличиваем соответсвующий коэффициент и в результате проскакиваем положение с нулевой ошибкой.