Здесь показаны различия между двумя версиями данной страницы.
stabilization_pid [2020/03/10 13:47] golikov |
stabilization_pid [2020/03/25 16:28] |
||
---|---|---|---|
Строка 1: | Строка 1: | ||
- | ===== ПИД Регулятор ===== | ||
- | Для регулирования объектами управления, как правило, используют типовые регуляторы, названия которых соответствуют названиям типовых звеньев: пропорциональный, интегрирующий, дифференцирующий. | ||
- | |||
- | ====П-регулятор, пропорциональный регулятор==== | ||
- | |||
- | Передаточная функция П-регулятора: Wп(s) = K1. | ||
- | Принцип действия заключается в том, что регулятор вырабатывает управляющее воздействие на объект пропорционально величине ошибки (чем больше ошибка, тем больше управляющее воздействие). | ||
- | |||
- | ====И-регулятор, интегрирующий регулятор==== | ||
- | |||
- | Передаточная функция И-регулятора: Wи(s) = К0/s. | ||
- | Управляющее воздействие пропорционально интегралу от ошибки. | ||
- | |||
- | ====Д-регулятор, дифференцирующий регулятор==== | ||
- | |||
- | Передаточная функция Д-регулятора: Wд(s) = К2*s. | ||
- | Д-регулятор генерирует управляющее воздействие только при изменении регулируемой величины: Y= K2 * dE/dt. | ||
- | |||
- | На практике данные простейшие П, И, Д регуляторы комбинируются в регуляторы вида ПИ, ПД, ПИД. | ||
- | |||
- | {{:lesson_pid_01.png?300|}} | ||
- | |||
- | В зависимости от выбранного вида регулятор может иметь пропорциональную характеристику (П), пропорционально-интегральную характеристику (ПИ), пропорционально-дифференциальную характеристику (ПД) или пропорционально-интегральную (изодромную) характеристику с воздействием по производной (ПИД-регулятор). | ||
- | |||
- | ПИ-регулятор, пропорционально-интегральный регулятор | ||
- | |||
- | ПИ-регулятор представляет собой сочетание П- и И-регуляторов. Передаточная функция ПИ-регулятора: Wпи(s) = K1 + K0/s. | ||
- | |||
- | ПД-регулятор, пропорционально-дифференциальный регулятор | ||
- | |||
- | ПД-регулятор представляет собой сочетание П- и Д-регуляторов. Передаточная функция ПД-регулятора: Wпд(s) = K1 + K2 s. | ||
- | |||
- | ПИД-регулятор, пропорционально-интегрально-дифференциальный регулятор | ||
- | |||
- | ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора: Wпид(s) = K1 + K0 / s + K2 s. | ||
- | |||
- | ====Код программы стабилизации на языке Python==== | ||
- | |||
- | <code python stabilization_PID.py> | ||
- | |||
- | |||
- | time_step = 0.1 # Временной шаг работы алгоритма, с | ||
- | omega_goal = 0.0 # Целевая угловая скорость спутника, град/с. Для режима стабилизации равна 0.0. | ||
- | mtr_num = 1 # Номер маховика | ||
- | mtr_max_speed = 3000 # Максимально допустимая скорость маховика, об/мин | ||
- | hyr_num = 1 # Номер ДУС (датчика угловой скорости) | ||
- | |||
- | error = 0 # Ошибка | ||
- | P = 0 # Воздействие пропорционального звена | ||
- | D = 0 # Воздействие дифференциального звена | ||
- | I = 0 # Воздействие интегрального звена | ||
- | Kp = 100 # Пропорциональный коэффициент | ||
- | Kd = 0.02 # Дифференциальный коэффициент | ||
- | Ki = 0.9 # Интегральный коэффициент | ||
- | lastError = 0 # Прошлая ошибка | ||
- | Integrator = 0 # Интеграл (сумма всех ошибок) | ||
- | PID = 0 # Величина управляющего воздействия | ||
- | Integrator_max = 10 # Ограничение максимального значения интеграла | ||
- | Integrator_min = -10 # Ограничение минимального значения интеграла | ||
- | |||
- | # Функции для определение новой скорости маховика. | ||
- | # mtr_speed - текущая угловая скорость маховика, об/мин | ||
- | # omega - текущая угловая скорость спутника, град/с | ||
- | # omega_goal - целевая угловая скорость спутника, град/с | ||
- | # mtr_new_speed - требуемая угловая скорость маховика, об/мин | ||
- | def motor_new_speed_PD(mtr_speed, omega, omega_goal): | ||
- | global Integrator | ||
- | global lastError | ||
- | error = omega - omega_goal # Вычисление ошибки | ||
- | P = Kp * error # Вычисление воздействия пропорционального звена | ||
- | D = Kd * ( error - lastError) / time_step # Вычисление воздействия дифференциального звена | ||
- | lastError = error # Запоминаем ошибку | ||
- | Integrator = Integrator + error * time_step # Накапливаем суммарную ошибку | ||
- | if Integrator > Integrator_max: # Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | ||
- | Integrator = Integrator_max | ||
- | elif Integrator < Integrator_min: | ||
- | Integrator = Integrator_min | ||
- | |||
- | I = Integrator * Ki # Вычисление воздействия интегрального звена | ||
- | PID = P + I + D # Вычисление суммарного управляющего воздействия | ||
- | |||
- | mtr_new_speed = int(mtr_speed + PID) | ||
- | if mtr_new_speed > mtr_max_speed: | ||
- | mtr_new_speed = mtr_max_speed | ||
- | elif mtr_new_speed < -mtr_max_speed: | ||
- | mtr_new_speed = -mtr_max_speed | ||
- | print "P= ", P, "I= ", I, "D= ", D, "PID= ", PID, "mtr_new_speed= ", mtr_new_speed | ||
- | return mtr_new_speed | ||
- | |||
- | # Функция включает все приборы, которые будут использоваться в основной программе. | ||
- | def initialize_all(): | ||
- | print "Enable motor №", mtr_num | ||
- | motor_turn_on(mtr_num) | ||
- | sleep(1) | ||
- | print "Enable angular velocity sensor №", hyr_num | ||
- | hyro_turn_on(hyr_num) | ||
- | sleep(1) | ||
- | |||
- | # Функция отключает все приборы, | ||
- | # которые будут использоваться в основной программе. | ||
- | def switch_off_all(): | ||
- | print "Finishing..." | ||
- | print "Disable angular velocity sensor №", hyr_num | ||
- | hyro_turn_off(hyr_num) | ||
- | motor_set_speed(mtr_num, 0) | ||
- | sleep (1) | ||
- | motor_turn_off(mtr_num) | ||
- | print "Finish program" | ||
- | |||
- | # Основная функция программы, в которой вызываются остальные функции. | ||
- | def control(): | ||
- | initialize_all() # Инициализируем все устройства | ||
- | mtr_state = 0 # Инициализируем статус маховика | ||
- | hyro_state = 0 # Инициализируем статус ДУС | ||
- | omega_goal = 0.0 # Целевой угол поворота | ||
- | |||
- | for i in range(100): | ||
- | print "i = ", i | ||
- | |||
- | # Опрос датчика угловой скорости и маховика. | ||
- | hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) | ||
- | mtr_state, mtr_speed = motor_request_speed(mtr_num) | ||
- | |||
- | # Обработка показаний датчика угловой скорости, | ||
- | # вычисление угловой скорости спутника по показаниям ДУС. | ||
- | # Если код ошибки ДУС равен 0, т.е. ошибки нет | ||
- | if not hyro_state: | ||
- | gx_degs = gx_raw * 0.00875 | ||
- | gy_degs = gy_raw * 0.00875 | ||
- | gz_degs = gz_raw * 0.00875 | ||
- | # если ДУС установлен осью z вверх, то угловая скорость | ||
- | # спутника совпадает с показаниями ДУС по оси z, иначе | ||
- | # необходимо изменить знак: omega = - gz_degs | ||
- | omega = gz_degs | ||
- | print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs | ||
- | elif hyro_state == 1: | ||
- | print "Fail because of access error, check the connection" | ||
- | elif hyro_state == 2: | ||
- | print "Fail because of interface error, check your code" | ||
- | |||
- | #Обработка показаний маховика и установка требуемой угловой скорости. | ||
- | if not mtr_state: # если код ошибки 0, т.е. ошибки нет | ||
- | print "Motor_speed: ", mtr_speed | ||
- | # установка новой скорости маховика | ||
- | mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal) | ||
- | motor_set_speed(mtr_num, mtr_new_speed) | ||
- | |||
- | sleep(time_step) | ||
- | |||
- | switch_off_all() | ||
- | </code> | ||
- | |||
- | ====Код программы стабилизации на языке Си==== | ||
- | |||
- | <code C stabilization_PID.с> | ||
- | #include <stdio.h> | ||
- | #include <stdint.h> | ||
- | #include "libschsat.h" | ||
- | #define LSS_OK 0 | ||
- | #define LSS_ERROR 1 | ||
- | #define LSS_BREAK 2 | ||
- | #include <math.h> | ||
- | |||
- | const float time_step = 100; // Временной шаг работы алгоритма, мс | ||
- | const int16_t mtr_max_speed = 3000; // Максимально допустимая скорость маховика, об/мин | ||
- | const uint16_t mtr_num = 1; // Номер маховика | ||
- | const uint16_t hyr_num = 1; // Номер ДУС | ||
- | const uint16_t mag_num = 1; // Номер магнитометра | ||
- | int16_t confirm = 0; //Переменная для функции motor_set_speed | ||
- | |||
- | float errora = 0; // Ошибка по углу | ||
- | float Pa = 0; // Воздействие пропорционального звена | ||
- | float Da = 0; // Воздействие дифференциального звена | ||
- | float Ia = 0; // Воздействие интегрального звена | ||
- | float Kpa = 20; // Пропорциональный коэффициент ошибки по углу | ||
- | float Kda = 0.1; // Дифференциальный коэффициент | ||
- | float Kia = 1; // Интегральный коэффициент | ||
- | float lastErrora = 0; // Прошлая ошибка по углу | ||
- | float Integratora = 0; // Интеграл (сумма всех ошибок по углу) | ||
- | float PID_alpha = 0; // Величина управляющего воздействия | ||
- | float Integrator_maxa = 10; // Ограничение максимального значения интеграла | ||
- | float Integrator_mina = -10; // Ограничение минимального значения интеграла | ||
- | |||
- | float error = 0; // Ошибка по угловой скорости | ||
- | float P = 0; // Воздействие пропорционального звена | ||
- | float D = 0; // Воздействие дифференциального звена | ||
- | float I = 0; // Воздействие интегрального звена | ||
- | float Kp = 20; // Пропорциональный коэффициент | ||
- | float Kd = 0.1; // Дифференциальный коэффициент | ||
- | float Ki = 1; // Интегральный коэффициент | ||
- | float lastError = 0; // Прошлая ошибка по угловой скорости | ||
- | float Integrator = 0; // Интеграл (сумма всех ошибок по угловой скорости) | ||
- | float PID_omega = 0; // Величина управляющего воздействия | ||
- | float Integrator_max = 10; // Ограничение максимального значения интеграла | ||
- | float Integrator_min = -10; // Ограничение минимального значения интеграла | ||
- | |||
- | int16_t mtr_new_speed; | ||
- | |||
- | int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ | ||
- | //Функция mag_calibrated вносит поправки в показания магнитометра с учетом калибровочных коэффициентов | ||
- | float magx_cal; | ||
- | magx_cal = 1.668867*(*magx + -38.326818) + -0.007649*(*magy + 20.442007) + 0.113348*(*magz + -69.659602); | ||
- | float magy_cal; | ||
- | magy_cal = -0.007649*(*magx + -38.326818) + 1.749597*(*magy + 20.442007) + 0.130999*(*magz + -69.659602); | ||
- | float magz_cal; | ||
- | magz_cal = 0.113348*(*magx + -38.326818) + 0.130999*(*magy + 20.442007) + 1.577785*(*magz + -69.659602); | ||
- | *magx = (int16_t) magx_cal; | ||
- | *magy = (int16_t) magy_cal; | ||
- | *magz = (int16_t) magz_cal; | ||
- | return 0; | ||
- | } | ||
- | |||
- | int motor_new_speed_PD(int mtr_speed, int alpha, int alpha_goal, int omega, int omega_goal){ | ||
- | /* Функция для определения новой скорости маховика. | ||
- | mtr_speed - текущая угловая скорость маховика, об/мин | ||
- | omega - текущая угловая скорость спутника, град/с | ||
- | omega_goal - целевая угловая скорость спутника, град/с | ||
- | mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ | ||
- | |||
- | error = omega - omega_goal; // Вычисление ошибки | ||
- | P = Kp * error; // Вычисление воздействия пропорционального звена | ||
- | D = Kd * (error - lastError) / time_step; // Вычисление воздействия дифференциального звена | ||
- | lastError = error; // Запоминаем ошибку | ||
- | Integrator = Integrator + error * time_step; // Накапливаем суммарную ошибку | ||
- | if (Integrator > Integrator_max){ // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | ||
- | Integrator = Integrator_max; | ||
- | } | ||
- | else if (Integrator < Integrator_min){ | ||
- | Integrator = Integrator_min; | ||
- | } | ||
- | I = Integrator * Ki; // Вычисление воздействия интегрального звена | ||
- | PID_omega = P + I + D; // Вычисление суммарного управляющего воздействия | ||
- | |||
- | errora = alpha - alpha_goal; // Вычисление ошибки | ||
- | Pa = Kpa * errora; // Вычисление воздействия пропорционального звена | ||
- | Da = Kda * (errora - lastErrora) / time_step; // Вычисление воздействия дифференциального звена | ||
- | lastErrora = errora; // Запоминаем ошибку | ||
- | Integratora = Integratora + errora * time_step; // Накапливаем суммарную ошибку | ||
- | if (Integratora > Integrator_maxa){ // Сатурация (Ограничиваем максимальное значение накапливаемой ошибки) | ||
- | Integratora = Integrator_maxa; | ||
- | } | ||
- | else if (Integratora < Integrator_mina){ | ||
- | Integratora = Integrator_mina; | ||
- | } | ||
- | Ia = Integratora * Kia; // Вычисление воздействия интегрального звена | ||
- | PID_alpha = Pa + Ia + Da; // Вычисление суммарного управляющего воздействия | ||
- | |||
- | mtr_new_speed = (int16_t)(mtr_speed + PID_omega + PID_alpha); | ||
- | |||
- | if (mtr_new_speed > mtr_max_speed){ | ||
- | mtr_new_speed = mtr_max_speed; | ||
- | } | ||
- | else if (mtr_new_speed < -mtr_max_speed){ | ||
- | mtr_new_speed = -mtr_max_speed; | ||
- | } | ||
- | printf("P=%f\t I=%f\t D=%f\t PID_omega=%f\t mtr_new_speed=%d\n", P, I, D, PID_omega, mtr_new_speed); | ||
- | printf("Pa=%f\t Ia=%f\t Da=%f\t PID_alpha=%f\t", Pa, Ia, Da, PID_alpha); | ||
- | return mtr_new_speed; | ||
- | } | ||
- | |||
- | int angle_transformation(int alpha, int alpha_goal){ | ||
- | if (alpha <= (alpha_goal - 180)){ | ||
- | alpha = alpha + 360; | ||
- | } | ||
- | else if (alpha>(alpha_goal +180)){ | ||
- | alpha = alpha - 360; | ||
- | } | ||
- | return alpha; | ||
- | } | ||
- | |||
- | void initialize_all(void){ | ||
- | /* Функция инициализации всех систем, | ||
- | включает все приборы,которые будут использоваться в основной программе.*/ | ||
- | printf ("Enable motor №%d\n", mtr_num); | ||
- | motor_turn_on(mtr_num); | ||
- | Sleep(1); | ||
- | printf("Enable angular velocity sensor №%d\n", hyr_num); | ||
- | hyro_turn_on(hyr_num); // Включаем ДУС | ||
- | Sleep(1); | ||
- | printf("Enable magnetometer %d\n", mag_num); | ||
- | magnetometer_turn_on(mag_num); | ||
- | Sleep(1); | ||
- | } | ||
- | |||
- | int switch_off_all(){ | ||
- | //Функция отключает все приборы,которые будут использоваться в основной программе. | ||
- | printf("Finishing...\n"); | ||
- | printf("Disable angular velocity sensor №%d\n", hyr_num); | ||
- | hyro_turn_off(hyr_num); | ||
- | printf("Disable magnetometer №%d\n", mag_num); | ||
- | magnetometer_turn_off(mag_num); | ||
- | motor_set_speed(mtr_num, 0, &confirm); | ||
- | Sleep (1); | ||
- | motor_turn_off(mtr_num); | ||
- | printf("Finish program\n"); | ||
- | return 0; | ||
- | } | ||
- | |||
- | int control(){ | ||
- | float mtr_new_speed; | ||
- | //данные магнитометра | ||
- | int16_t mgx_cal=0; | ||
- | int16_t mgy_cal=0; | ||
- | int16_t mgz_cal=0; | ||
- | int16_t *magx_raw = &mgx_cal; | ||
- | int16_t *magy_raw = &mgy_cal; | ||
- | int16_t *magz_raw = &mgz_cal; | ||
- | //данные ДУС | ||
- | int16_t gx_raw; | ||
- | int16_t gy_raw; | ||
- | int16_t gz_raw; | ||
- | int16_t *hyrox_raw = &gx_raw; | ||
- | int16_t *hyroy_raw = &gy_raw; | ||
- | int16_t *hyroz_raw = &gz_raw; | ||
- | int16_t mtr_speed; | ||
- | int16_t omega = 0; | ||
- | int16_t omega_goal = 0; // Целевая угловая скорость | ||
- | |||
- | initialize_all(); | ||
- | |||
- | int mtr_state = 0; // Инициализируем статус маховика | ||
- | int hyro_state = 0; // Инициализируем статус ДУС | ||
- | int mag_state = 0; // Инициализируем статус магнитометра | ||
- | int alpha_goal = 90; // Целевой угол | ||
- | double mag_alpha = 0; | ||
- | int i; | ||
- | for (i = 0; i < 120; i++){ | ||
- | mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw); | ||
- | hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); | ||
- | gx_raw = *hyrox_raw; | ||
- | gy_raw = *hyroy_raw; | ||
- | gz_raw = *hyroz_raw; | ||
- | mtr_state = motor_request_speed(mtr_num, &mtr_speed); | ||
- | if (!mag_state){ | ||
- | mag_calibrated(magx_raw,magy_raw,magz_raw); | ||
- | /* *magy_raw = - *magy_raw; переходим из левой системы координат, | ||
- | которая изображена на магнитометре в правую, для того чтобы | ||
- | положительное направление угла было против часовой стрелки*/ | ||
- | mag_alpha = atan2(mgy_cal, mgx_cal)/M_PI*180; | ||
- | mag_alpha = angle_transformation(mag_alpha, alpha_goal); | ||
- | //printf("magx_cal = %d", mgx_cal); | ||
- | //printf(" magy_cal = %d", mgy_cal); | ||
- | //printf(" magz_cal = %d", mgz_cal); | ||
- | printf("mag_alpha = %f", mag_alpha); | ||
- | } | ||
- | else if (mag_state == 1){ | ||
- | printf("Fail because of access error, check the connection\n"); | ||
- | } | ||
- | else if (mag_state == 2){ | ||
- | printf("Fail because of interface error, check your code\n"); | ||
- | } | ||
- | |||
- | if (!hyro_state){ | ||
- | //float gx_degs = gx_raw * 0.00875; | ||
- | //float gy_degs = gy_raw * 0.00875; | ||
- | float gz_degs = gz_raw * 0.00875; | ||
- | /* если ДУС установлен осью z вверх, то угловая скорость | ||
- | // спутника совпадает с показаниями ДУС по оси z, иначе | ||
- | необходимо изменить знак: omega = - gz_degs */ | ||
- | omega = gz_degs; | ||
- | //printf("\ngx_degs = %f", gx_degs); | ||
- | //printf(" gy_degs = %f", gy_degs); | ||
- | printf(" gz_degs = %f\n", gz_degs); | ||
- | } | ||
- | else if (hyro_state == 1){ | ||
- | printf("Fail because of access error, check the connection\n"); | ||
- | } | ||
- | else if (hyro_state == 2){ | ||
- | printf("Fail because of interface error, check your code\n"); | ||
- | } | ||
- | |||
- | //Обработка показаний маховика и установка требуемой угловой скорости. | ||
- | if (!mtr_state) { | ||
- | // если код ошибки 0, т.е. ошибки нет | ||
- | int16_t mtr_speed=0; | ||
- | motor_request_speed(mtr_num, &mtr_speed); | ||
- | //printf("\nMotor_speed: %d\n", mtr_speed); | ||
- | // установка новой скорости маховика | ||
- | mtr_new_speed = motor_new_speed_PD(mtr_speed, mag_alpha, alpha_goal, omega, omega_goal); | ||
- | motor_set_speed(mtr_num, mtr_new_speed, &confirm); | ||
- | } | ||
- | mSleep(time_step); | ||
- | } | ||
- | switch_off_all(); | ||
- | return 0; | ||
- | } | ||
- | </code> |