Инструменты пользователя

Инструменты сайта


stabilization_pid

Различия

Здесь показаны различия между двумя версиями данной страницы.

Ссылка на это сравнение

Предыдущая версия справа и слева Предыдущая версия
Следующая версия
Предыдущая версия
stabilization_pid [2020/03/10 13:31]
golikov
stabilization_pid [2020/03/10 13:56]
golikov
Строка 36: Строка 36:
 ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора:​ Wпид(s) = K1 + K0 / s + K2 s. ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора:​ Wпид(s) = K1 + K0 / s + K2 s.
  
 +====Код программы стабилизации на языке Python====
  
 <code python stabilization_PID.py>​ <code python stabilization_PID.py>​
  
-# Временной шаг работы алгоритма,​ с + 
-time_step ​= 0.+time_step = 0.1 # Временной шаг работы алгоритма,​ с 
-# Целевая угловая скорость спутника,​ град/​с. +omega_goal ​= 0.0 # Целевая угловая скорость спутника,​ град/​с. Для режима стабилизации равна 0.0. 
-Для режима стабилизации равна 0.0. +mtr_num ​1 # Номер маховика 
-omega_goal ​0.0 +mtr_max_speed ​3000 # Максимально допустимая скорость маховика,​ об/​мин 
-# Номер маховика +hyr_num ​1 # Номер ДУС (датчика угловой скорости)
-mtr_num ​+
-# Максимально допустимая скорость маховика,​ об/​мин +
-mtr_max_speed ​3000 +
-# Номер ДУС (датчика угловой скорости) +
-hyr_num = 1+
  
 error = 0      # Ошибка error = 0      # Ошибка
Строка 61: Строка 57:
 Integrator = 0  # Интеграл (сумма всех ошибок) Integrator = 0  # Интеграл (сумма всех ошибок)
 PID = 0    # Величина управляющего воздействия PID = 0    # Величина управляющего воздействия
-Integrator_max = 10 # +Integrator_max = 10 # Ограничение максимального значения интеграла ​ 
-Integrator_min = -10 # Ограничение минимального ​+Integrator_min = -10 # Ограничение минимального ​значения интеграла ​
  
 # Функции для определение новой скорости маховика. # Функции для определение новой скорости маховика.
-# Новая скорость маховика складывается из 
-# текущей скорости маховика и приращения скорости. 
-# Приращение скорости пропорционально ошибке по углу 
-# и ошибке по угловой скорости. 
 # mtr_speed - текущая угловая скорость маховика,​ об/мин # mtr_speed - текущая угловая скорость маховика,​ об/мин
 # omega - текущая угловая скорость спутника,​ град/с # omega - текущая угловая скорость спутника,​ град/с
Строка 97: Строка 89:
  return mtr_new_speed  return mtr_new_speed
    
-# Функция включает все приборы,​ +# Функция включает все приборы,​ которые будут использоваться в основной программе.
-которые будут использоваться в основной программе.+
 def initialize_all():​ def initialize_all():​
  print "​Enable motor №", mtr_num ​  print "​Enable motor №", mtr_num ​
Строка 120: Строка 111:
 # Основная функция программы,​ в которой вызываются остальные функции. # Основная функция программы,​ в которой вызываются остальные функции.
 def control(): def control():
- initialize_all() + initialize_all() # Инициализируем все устройства 
- # Инициализируем статус маховика + mtr_state = 0 # Инициализируем статус маховика 
- mtr_state ​= 0 + hyro_state ​= 0 # Инициализируем статус ДУС 
- # Инициализируем статус ДУС  +        omega_goal = 0.0 # Целевой угол поворота
- hyro_state = 0 +
-        omega_goal = 0.0+
                    
  for i in range(100):  for i in range(100):
Строка 163: Строка 152:
 </​code>​ </​code>​
  
-<​code ​с stabilization_PID.с>​+====Код программы стабилизации на языке Си==== 
 + 
 +<​code ​stabilization_PID.с>​
 #include <​stdio.h>​ #include <​stdio.h>​
 #include <​stdint.h>​ #include <​stdint.h>​
Строка 173: Строка 164:
  
 const float time_step = 100; // Временной шаг работы алгоритма,​ мс const float time_step = 100; // Временной шаг работы алгоритма,​ мс
-//const float omega_goal = 0.0; // Целевая угловая скорость спутника,​ град/​с. Для режима стабилизации равна 0.0. 
-//const float alpha_goal = 0; // Целевой угол поворота 
 const int16_t mtr_max_speed = 3000; // Максимально допустимая скорость маховика,​ об/мин const int16_t mtr_max_speed = 3000; // Максимально допустимая скорость маховика,​ об/мин
-const uint16_t mtr_num = 1; // Номер маховика +const uint16_t mtr_num = 1; // Номер маховика 
-const uint16_t hyr_num = 1; // Номер ДУС +const uint16_t hyr_num = 1; // Номер ДУС 
-const uint16_t mag_num = 1; // Номер магнитометра +const uint16_t mag_num = 1; // Номер магнитометра
-//float mag_alpha = 0; +
-//float omega = 0;+
 int16_t confirm = 0;​ //​Переменная для функции motor_set_speed int16_t confirm = 0;​ //​Переменная для функции motor_set_speed
  
 float errora = 0;            // Ошибка по углу float errora = 0;            // Ошибка по углу
-float Pa = 0;         // Воздействие пропорционального звена +float Pa = 0;         // Воздействие пропорционального звена 
-float Da = 0;         // Воздействие дифференциального звена +float Da = 0;         // Воздействие дифференциального звена 
-float Ia = 0;         // Воздействие интегрального звена +float Ia = 0;         // Воздействие интегрального звена 
-float Kpa = 20; // Пропорциональный коэфициент ошибки по углу+float Kpa = 20; // Пропорциональный коэффициент ошибки по углу
 float Kda = 0.1;            // Дифференциальный коэффициент float Kda = 0.1;            // Дифференциальный коэффициент
 float Kia = 1;            // Интегральный коэффициент float Kia = 1;            // Интегральный коэффициент
Строка 193: Строка 180:
 float Integratora = 0;      // Интеграл (сумма всех ошибок по углу) float Integratora = 0;      // Интеграл (сумма всех ошибок по углу)
 float PID_alpha = 0;            // Величина управляющего воздействия float PID_alpha = 0;            // Величина управляющего воздействия
-float Integrator_maxa = 10;  // Ограничение максимального значения интергатора +float Integrator_maxa = 10;  // Ограничение максимального значения интеграла 
-float Integrator_mina = -10; // Ограничение минимального значения интергатора+float Integrator_mina = -10; // Ограничение минимального значения интеграла
  
-float error = 0;           // Ошибка по угловой скорости +float error = 0;           // Ошибка по угловой скорости 
-float P = 0;         // Воздействие пропорционального звена +float P = 0;         // Воздействие пропорционального звена 
-float D = 0;         // Воздействие дифференциального звена +float D = 0;         // Воздействие дифференциального звена 
-float I = 0;         // Воздействие интегрального звена +float I = 0;         // Воздействие интегрального звена 
-float Kp = 20;             // Пропорциональный коэффициент +float Kp = 20;             // Пропорциональный коэффициент 
-float Kd = 0.1;           // Дифференциальный коэффициент +float Kd = 0.1;           // Дифференциальный коэффициент 
-float Ki = 1;             // Интегральный коэффициент +float Ki = 1;             // Интегральный коэффициент 
-float lastError = 0;       // Прошлая ошибка по угловой скорости +float lastError = 0;       // Прошлая ошибка по угловой скорости 
-float Integrator = 0;       // Интеграл (сумма всех ошибок по угловой скорости) +float Integrator = 0;       // Интеграл (сумма всех ошибок по угловой скорости) 
-float PID_omega = 0;       // Величина управляющего воздействия +float PID_omega = 0;       // Величина управляющего воздействия 
-float Integrator_max = 10;  // Ограничение максимального значения интергатора +float Integrator_max = 10;  // Ограничение максимального значения интеграла 
-float Integrator_min = -10; // Ограничение минимального значения интергатора+float Integrator_min = -10; // Ограничение минимального значения интеграла
  
 int16_t mtr_new_speed;​ int16_t mtr_new_speed;​
Строка 213: Строка 200:
 int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){
 //​Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов //​Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов
- float magx_cal; ​ // коэффициенты для орбикрафт 1 собраны у моего стола для SCHSAT_SX01+ float magx_cal;
  magx_cal = 1.668867*(*magx + -38.326818) + -0.007649*(*magy + 20.442007) + 0.113348*(*magz + -69.659602);​  magx_cal = 1.668867*(*magx + -38.326818) + -0.007649*(*magy + 20.442007) + 0.113348*(*magz + -69.659602);​
  float magy_cal;  float magy_cal;
Строка 226: Строка 213:
  
 int motor_new_speed_PD(int mtr_speed, int alpha, int alpha_goal, int omega, int omega_goal){ int motor_new_speed_PD(int mtr_speed, int alpha, int alpha_goal, int omega, int omega_goal){
- /* Функция для определения новой скорости маховика+ /* Функция для определения новой скорости маховика.
- ​Новая скорость маховика складывается из +
- ​текущей скорости маховика и приращения скорости. +
- ​Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости.+
  ​mtr_speed - текущая угловая скорость маховика,​ об/мин  ​mtr_speed - текущая угловая скорость маховика,​ об/мин
  omega - текущая угловая скорость спутника,​ град/с  omega - текущая угловая скорость спутника,​ град/с
  ​omega_goal - целевая угловая скорость спутника,​ град/с  ​omega_goal - целевая угловая скорость спутника,​ град/с
  ​mtr_new_speed - требуемая угловая скорость маховика,​ об/​мин*/​  ​mtr_new_speed - требуемая угловая скорость маховика,​ об/​мин*/​
-  +   
- //​mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal));​  + error = omega - omega_goal; ​          // Вычисление ошибки 
-  + P = Kp * error; ​                      // Вычисление воздействия пропорционального звена 
- error = omega - omega_goal; ​          // Вычисление ошибки + D = Kd * (error - lastError) / time_step;​ //​ Вычисление воздействия дифференциального звена 
- P = Kp * error; ​                      // Вычисление воздействия пропорционального звена + lastError = error; ​                    // Запоминаем ошибку
- D = Kd * (error - lastError) / time_step; // Вычисление воздействия дифференциального звена +
- lastError = error; ​                    // Запоминаем ошибку+
  Integrator = Integrator + error * time_step; // Накапливаем суммарную ошибку  Integrator = Integrator + error * time_step; // Накапливаем суммарную ошибку
- if (Integrator > Integrator_max){ ​        // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)+ if (Integrator > Integrator_max){ ​        // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
  Integrator = Integrator_max;​  Integrator = Integrator_max;​
  }  }
Строка 248: Строка 230:
  Integrator = Integrator_min;​  Integrator = Integrator_min;​
  }  }
- I = Integrator * Ki;             // Вычисление воздействия интегрального звена + I = Integrator * Ki;             // Вычисление воздействия интегрального звена 
- PID_omega = P + I + D;     // Вычисление суммарного управляющего воздействия+ PID_omega = P + I + D;     // Вычисление суммарного управляющего воздействия
   
- errora = alpha - alpha_goal; ​          // Вычисление ошибки + errora = alpha - alpha_goal; ​          // Вычисление ошибки 
- Pa = Kpa * errora; ​                      // Вычисление воздействия пропорционального звена+ Pa = Kpa * errora; ​                      // Вычисление воздействия пропорционального звена
  Da = Kda * (errora - lastErrora) / time_step;​ //​ Вычисление воздействия дифференциального звена  Da = Kda * (errora - lastErrora) / time_step;​ //​ Вычисление воздействия дифференциального звена
- lastErrora = errora; ​                    // Запоминаем ошибку + lastErrora = errora; ​                    // Запоминаем ошибку 
- Integratora = Integratora + errora * time_step; ​ // Накапливаем суммарную ошибку+ Integratora = Integratora + errora * time_step; // Накапливаем суммарную ошибку
  if (Integratora > Integrator_maxa){ ​         // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)  if (Integratora > Integrator_maxa){ ​         // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки)
  Integratora = Integrator_maxa;​  Integratora = Integrator_maxa;​
Строка 262: Строка 244:
  Integratora = Integrator_mina;​  Integratora = Integrator_mina;​
  }  }
- Ia = Integratora * Kia;             // Вычисление воздействия интегрального звена + Ia = Integratora * Kia;             // Вычисление воздействия интегрального звена 
- PID_alpha = Pa + Ia + Da;     // Вычисление суммарного управляющего воздействия + PID_alpha = Pa + Ia + Da;     // Вычисление суммарного управляющего воздействия
  
  mtr_new_speed = (int16_t)(mtr_speed + PID_omega + PID_alpha);  mtr_new_speed = (int16_t)(mtr_speed + PID_omega + PID_alpha);
Строка 289: Строка 271:
  
 void initialize_all(void){ void initialize_all(void){
-/*  Функция инициализации всех систем,​+/​* Функция инициализации всех систем,​
  включает все приборы,​которые будут использоваться в основной программе.*/​  включает все приборы,​которые будут использоваться в основной программе.*/​
  printf ("​Enable motor №%d\n",​ mtr_num);  printf ("​Enable motor №%d\n",​ mtr_num);
Строка 303: Строка 285:
  
 int switch_off_all(){ int switch_off_all(){
-/*Функция отключает все приборы,​которые будут использоваться в основной программе.*/+//Функция отключает все приборы,​которые будут использоваться в основной программе.
  printf("​Finishing...\n"​);​  printf("​Finishing...\n"​);​
  printf("​Disable angular velocity sensor №%d\n",​ hyr_num);  printf("​Disable angular velocity sensor №%d\n",​ hyr_num);
Строка 317: Строка 299:
  
 int control(){ int control(){
- float mtr_new_speed;​ 
  //​данные магнитометра  //​данные магнитометра
  int16_t mgx_cal=0;  int16_t mgx_cal=0;
Строка 338: Строка 319:
  initialize_all();​  initialize_all();​
   
- int mtr_state = 0; // Инициализируем статус маховика+ int mtr_state = 0; // Инициализируем статус маховика
  int hyro_state = 0; // Инициализируем статус ДУС  int hyro_state = 0; // Инициализируем статус ДУС
- int mag_state = 0;  // Инициализируем статус магнитометра+ int mag_state = 0;  // Инициализируем статус магнитометра
  int alpha_goal = 90; // Целевой угол   int alpha_goal = 90; // Целевой угол
  double mag_alpha = 0;  double mag_alpha = 0;
stabilization_pid.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

Инструменты страницы