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

Перевод этой страницы:

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


Боковая панель

Для чего нужен ОрбиКрафт

Подсистемы конструктора

Инструкции по работе с ОрбиКрафт

Уроки

Лабораторная оснастка

Знакомство с Arduino

Полезная нагрузка на базе Arduino

Обратная связь

Новости

stabilization_pid

Это старая версия документа!


ПИД Регулятор

Для регулирования объектами управления, как правило, используют типовые регуляторы, названия которых соответствуют названиям типовых звеньев: пропорциональный, интегрирующий, дифференцирующий.

П-регулятор, пропорциональный регулятор

Передаточная функция П-регулятора: Wп(s) = K1. Принцип действия заключается в том, что регулятор вырабатывает управляющее воздействие на объект пропорционально величине ошибки (чем больше ошибка, тем больше управляющее воздействие).

И-регулятор, интегрирующий регулятор

Передаточная функция И-регулятора: Wи(s) = К0/s. Управляющее воздействие пропорционально интегралу от ошибки.

Д-регулятор, дифференцирующий регулятор

Передаточная функция Д-регулятора: Wд(s) = К2*s. Д-регулятор генерирует управляющее воздействие только при изменении регулируемой величины: Y= K2 * dE/dt.

На практике данные простейшие П, И, Д регуляторы комбинируются в регуляторы вида ПИ, ПД, ПИД.

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

ПИ-регулятор, пропорционально-интегральный регулятор

ПИ-регулятор представляет собой сочетание П- и И-регуляторов. Передаточная функция ПИ-регулятора: Wпи(s) = K1 + K0/s.

ПД-регулятор, пропорционально-дифференциальный регулятор

ПД-регулятор представляет собой сочетание П- и Д-регуляторов. Передаточная функция ПД-регулятора: Wпд(s) = K1 + K2 s.

ПИД-регулятор, пропорционально-интегрально-дифференциальный регулятор

ПИД-регулятор представляет собой сочетание П-, И- и Д-регуляторов. Передаточная функция ПИД-регулятора: Wпид(s) = K1 + K0 / s + K2 s.

Код программы стабилизации на языке 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()

Код программы стабилизации на языке Си

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

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