Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Для чего нужен ОрбиКрафт
Подсистемы конструктора
Инструкции по работе с ОрбиКрафт
Уроки
Лабораторная оснастка
Знакомство с Arduino
Полезная нагрузка на базе Arduino
Обратная связь
Новости
Режим стабилизации спутника означает поддержание нулевой угловой скорости. Такой режим необходим, например для получения четких снимков или их передачи на наземный пункт приема, когда время передачи данных продолжительно и не допустимо отклонение антенны спутника от направления на наземный пункт приема данных. Также описанная в этом уроке теория подходит для режима поддержания любой требуемой угловой скорости, а не только нулевой, для таких задач как отслеживание подвижного объекта.
Изменять угловую скорость спутника можно с помощью маховиков, реактивных двигателей, электромагнитных катушек, двигателей - гиродинов. В этом примере мы рассмотрим управление управляющим моментом с помощью маховика. Действие этого устройства основано на законе сохранения момента импульса. Например, когда двигатель-маховик раскручивается в одну сторону, то космический аппарат (КА), соответственно, начинает вращаться в другую сторону под действием такого же раскручивающего момента, но направленного в противоположную сторону в соответствии с третьим законом Ньютона. Если под влиянием внешних факторов КА начал разворачиваться в определённом направлении, достаточно увеличить скорость вращения маховика в ту же сторону и нежелательный поворот КА прекратится, вместо спутника вращательный момент «вберет» в себя маховик. Получать информацию об угловой скорости спутника будем с помощью датчика угловой скорости. В этом примере мы рассмотрим как по показаниям датчика угловой скорости и данных о частоте вращения маховика рассчитать управляющие команды для маховика, чтобы спутник был стабилизирован или поддерживал требуемую угловую скорость.
Аналогом закона сохранения импульса для вращательного движения является закон сохранения момента импульса или закон сохранения кинетического момента:
$\sum\limits_{i=1}^{n}{{{J}_{i}}\cdot {{\omega }_{i}}}=const \label{eq:1}$
Вообще вращательное движение спутника описывается законами схожими с законами известными для поступательного движения. Так, например, для каждого параметра в поступательном движении есть аналогичный параметр для вращательного движения:
Поступательное движение | Аналогия | Вращательное движение |
---|---|---|
Сила | $F\leftrightarrow M$ | Момент |
Расстояние | $S\leftrightarrow \alpha$ | Угол |
Скорость | $V\leftrightarrow\omega$ | Угловая скорость |
Ускорение | $a\leftrightarrow\epsilon$ | Угловое ускорение |
Масса | $m\leftrightarrow J$ | Момент инерции |
Законы движения также выглядят аналогичным образом
Название закона | Поступательное движение | Вращательное движение |
---|---|---|
второй закон Ньютона | $F=m\cdot a$ | $M=J\cdot \epsilon$ |
кинетическая энергия | $E=\frac{m\cdot {{V}^{2}}}{2}$ | $E=\frac{J\cdot {{\omega}^{2}}}{2}$ |
закон сохранения количества движения | $\sum\limits_{i=1}^{n}{{{m}_{i}}\cdot {{V }_{i}}}=const$ | $\sum\limits_{i=1}^{n}{{{J}_{i}}\cdot {{\omega }_{i}}}=const$ |
Запишем закон сохранения кинетического момента системы спутник+маховик для моментов времени «1» и «2»:
${{J}_{s}}\cdot {{\omega }_{s1}}+{{J}_{m}}\cdot {{\omega }_{m1}}={{J}_{s}}\cdot {{\omega }_{s2}}+{{J}_{m}}\cdot {{\omega }_{m2}}$
Абсолютная скорость маховика, т.е. скорость маховика в инерциальной системе координат, например связанной с Землей, представляет собой сумму угловой скорости спутника и угловой скорости маховика относительно спутника, т.е. относительной угловой скорости маховика:
${{\omega }_{mi}}={{\omega }_{si}}+{{{\omega }'}_{mi}}$
Обращаем внимание, что маховик может измерять собственную угловую скорость относительно корпуса спутника или относительную угловую скорость.
Выразим угловую скорость маховика, которую необходимую задать
${{J}_{s}}\cdot {{\omega }_{s1}}+{{J}_{m}}\cdot \left( {{\omega }_{s1}}+{{{{\omega }'}}_{m1}} \right)={{J}_{s}}\cdot {{\omega }_{s2}}+{{J}_{m}}\cdot \left( {{\omega }_{s2}}+{{{{\omega }'}}_{m2}} \right) $
$ \left( {{J}_{s}}+{{J}_{m}} \right)\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right)=-{{J}_{m}}({{\omega }_{m1}}-{{\omega }_{m2}}) $
$ {{\omega }_{m2}}={{\omega }_{m1}}+\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right) $
Обозначим отношение $\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}$ как $k_d$.
Для работы алгоритма необязательно знать точное значение $\frac{{{J}_{s}}+{{J}_{m}}}{{{J}_{m}}}$, т.к. маховик не может мгновенно установить требуемую угловую скорость. Также в процесс управления вмешиваются шумы измерений: угловая скорость спутника измеренная с помощью датчика угловой скорости не является точной, т.к. в измерениях всегда есть постоянная ошибка и шум измерений. Следует учесть, что измерения угловой скорости и выдача команд маховику происходят с некоторым минимальным шагом во времени. Все эти ограничения приводят к тому, что $k_d$ подбирается экспериментальным путем или строятся подробные компьютерные модели, которые учитывают все вышеописанные ограничения. В нашем случае коэффициент $k_d$ будем подбирать экспериментально.
$ {{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\left( {{\omega }_{s1}}-{{\omega }_{s2}} \right) $
Угловая скорость $\omega_{s2}$ в момент времени «2» является целевой угловой скоростью, обозначим ее $\omega_{s\_goal}$. Таким образом, если необходимо что спутник поддерживал угловую скорость $\omega_{s\_goal}$, то зная текущую угловую скорость спутника и текущую угловую скорость маховика возможно рассчитать желаемую скорость маховика для поддержания режима «вращения с постоянной скоростью»:
${{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\left( {{\omega }_{s1}}-{{\omega }_{{s\_goal}}} \right)$
Используя режим вращения с постоянной скоростью можно заставить спутник повернуться на любой угол если вращать спутник с постоянной скоростью определенное время. Тогда время, которое необходимо вращать спутник с постоянной скоростью $\omega_{s\_goal}$, чтобы развернуться на требуемый угол $\alpha$ определяется делением этих величин:
$t=\frac{\alpha}{\omega_{{s\_goal}}}$
Если требуется чтобы спутник был застабилизирован, то $\omega_{s\_goal}=0$ и выражение приобретает более простой вид:
${{\omega }_{m2}}={{\omega }_{m1}}+{{k}_{d}}\cdot {{\omega }_{s1}}$
# запрос данных угловой скорости спутника (ДУС) и маховика hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) mtr_state, mtr_speed = motor_request_speed(mtr_num) # перевод значений угловой скорости в градусы/сек gx_degs = gx_raw * 0.00875 # если ДУС установлен осью z вверх, то угловая скорость # спутника совпадает с показаниями ДУС по оси z, иначе # необходимо изменить знак: omega = - gz_degs omega = gz_degs mtr_new_speed = int(mtr_speed+ kd*(omega-omega_goal))
# Коэффициент дифференциальной обратной связи. # Коэффициент положительный, если маховик расположен осью z вверх # и ДУС расположен осью z также вверх. # Коэффициент подбирается экспериментально в зависимости от формы # и массы вашего спутника. kd = 200.0 # Временной шаг работы алгоритма, с time_step = 0.1 # Целевая угловая скорость спутника, град/с. # Для режима стабилизации равна 0.0. omega_goal = 0.0 # Номер маховика mtr_num = 1 # Максимально допустимая скорость маховика, об/мин mtr_max_speed = 5000 # Номер ДУС (датчика угловой скорости) hyr_num = 1 # Функции для определение новой скорости маховика. # Новая скорость маховика складывается из # текущей скорости маховика и приращения скорости. # Приращение скорости пропорционально ошибке по углу # и ошибке по угловой скорости. # mtr_speed - текущая угловая скорость маховика, об/мин # omega - текущая угловая скорость спутника, град/с # omega_goal - целевая угловая скорость спутника, град/с # mtr_new_speed - требуемая угловая скорость маховика, об/мин def motor_new_speed_PD(mtr_speed, omega, omega_goal): mtr_new_speed = int(mtr_speed + kd*(omega-omega_goal)) 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 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(1000): 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()
#include "libschsat.h" const float kd = 200.0; // Временной шаг работы алгоритма, с const float time_step = 0.2; // Целевая угловая скорость спутника, град/с. // Для режима стабилизации равна 0.0. const float omega_goal = 0.0; // Номер маховика const int mtr_num = 1; // Максимально допустимая скорость маховика, об/мин const int mtr_max_speed = 5000; // Номер ДУС (датчика угловой скорости) const uint16_t hyr_num = 1; int16_t mtr_new_speed; int motor_new_speed_PD(int mtr_speed, int omega, float omega_goal){ /* Функция для определения новой скорости маховика. Новая скорость маховика складывается из текущей скорости маховика и приращения скорости. Приращение скорости пропорционально ошибке по углу и ошибке по угловой скорости. mtr_speed - текущая угловая скорость маховика, об/мин omega - текущая угловая скорость спутника, град/с omega_goal - целевая угловая скорость спутника, град/с mtr_new_speed - требуемая угловая скорость маховика, об/мин*/ mtr_new_speed = (int)(mtr_speed + kd * (omega - omega_goal)); 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; } return mtr_new_speed; } 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); } void switch_off_all(void){ /* Функция отключает все приборы,которые будут использоваться в основной программе.*/ printf("Finishing..."); int16_t new_speed = mtr_new_speed; // printf("\nDisable angular velocity sensor №%d\n", hyr_num); hyro_turn_off(hyr_num); motor_set_speed(mtr_num, 0, &new_speed); Sleep (1); motor_turn_off(mtr_num); printf("Finish program\n"); } void control(void){ initialize_all(); // Инициализируем статус маховика int16_t mtr_state; // Инициализируем статус ДУС int16_t hyro_state = 0; int16_t pRAW_dataX = 0; int16_t pRAW_dataY = 0; int16_t pRAW_dataZ = 0; int16_t *gx_raw = &pRAW_dataX; int16_t *gy_raw = &pRAW_dataY; int16_t *gz_raw = &pRAW_dataZ; int16_t speed = 0; int16_t *mtr_speed = &speed; int16_t omega; int i; for(i = 0; i < 1000; i++){ printf("i = %d\n", i); // Опрос датчика угловой скорости и маховика. hyro_state = hyro_request_raw(hyr_num, gx_raw, gy_raw, gz_raw); mtr_state = motor_request_speed(mtr_num, mtr_speed); // Обработка показаний датчика угловой скорости, // вычисление угловой скорости спутника по показаниям ДУС. // Если код ошибки ДУС равен 0, т.е. ошибки нет 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("gx_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"); } int mtr_new_speed; //Обработка показаний маховика и установка требуемой угловой скорости. if (!mtr_state) { // если код ошибки 0, т.е. ошибки нет int16_t mtr_speed=0; motor_request_speed(mtr_num, &mtr_speed); printf("Motor_speed: %d\n", mtr_speed); // установка новой скорости маховика mtr_new_speed = motor_new_speed_PD(mtr_speed,omega,omega_goal); motor_set_speed(mtr_num, mtr_new_speed, &omega); } } Sleep(time_step); switch_off_all(); }