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

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


stabilization_pid

Различия

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

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

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>​ 
stabilization_pid.txt · Последние изменения: 2020/03/25 16:28 (внешнее изменение)

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